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1.0 


INTRODUCTION 


In  recent  years  the  techniques  of  estimation  theory  have  been  applied  to 
many  problems  associated  with  ballistic  missile  flight  testing.  In  particular, 
trajectory  reconstruction,  weapon  system  analysis,  metric  sensor  evaluation, 
and  geodetic  and  geopotential  model  refinement  are  examples  of  problem  areas 
to  which  estimation  theory  has  been  successfully  applied. 

In  practice,  estimation  theory  is  used  in  two  ways;  namely  to  estimate  various 
trajectory  and  system  parameters  using  available  measurement  data,  and  to 
perform  an  error  analysis  of  the  estimation  process  itself. 

Test  range  activities,  such  as  pre-mission  planning  and  post-mission  analysis, 
are  strongly  influenced  by  requirements  imposed  for  trajectory  reconstruction 
and  system  performance  analysis.  Planning  for  mission  support  usually  involves 
error  analyses  based  on  alternative  range  instrumentation  configurations. 
Post-mission  activities  include  estimation  of  trajectory  and  system  parameters 
and  associated  error  analyses. 

Future  test  range  activity  is  projected  to  involve  more  accurate  guidance 
systems,  advanced  reentry  systems,  and  range  instrumentation  with  more 
accuracy  and  precision.  The  ability  to  meet  future  range  testing  requirements 
will  depend  to  a great  extent  on  the  fidelity  and  generality  of  the  estimation 
techniques  employed. 

Computer  programs  currently  in  use  at  SAMTEC  for  estimation  and  error  analysis 
are  of  the  batch  processing  variety  and,  as  such,  have  certain  deficiencies. 

In  the  first  place,  whenever  the  trajectory  constraints  are  noisy,  such  as 
when  derived  from  noisy  guidance  data  or  during  uninstrumented  reentry,  it  is 
not  practical  to  perform  optimum  trajectory  reconstruction  with  batch  proces- 
sing techniques.  Secondly,  the  types  of  estimation  problems  which  arise  in 
connection  with  range  operations  involve  nonlinear  equations,  and  the  iterative 
methods  of  solution  these  problems  require  often  converge  slowly,  or  even 
diverge,  when  implemented  in  a batch  processor. 
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The  alternative  to  batch  processing  Is  recursive  processing  which  not  only 
admits  optimum  trajectory  reconstruction  when  the  trajectory  constraints 
are  noisy,  but  also  provides  excellent  convergence  properties  which  can 
result  In  considerable  computational  superiority  over  batch  processing  when 
the  latter  requires  many  Iterations  [1]. 

Because  of  the  limitations  of  existing  programs  vis-a-vis  projected  future 
requirements,  an  effort  has  been  initiated  at  SAMTEC  to  develop  a new  pro- 
gram (TRAM)  to  meet  these  requirements.  The  TRAM  program  employs  recursive 
processing  so  that  more  general  optimum  estimation  techniques  can  be  imple- 
mented and  better  convergence  properties  achieved  than  are  possible  with 
batch  processing. 
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2.0 


SCOPE  OF  THE  REPORT 


In  Section  3.0  of  the  report,  a system  viewpoint  is  adopted  for  the  aggregate 
functions  of  tracking  and  estimation.  This  viewpoint  provides  insight  which 
is  useful  in  estimator  implementation. 

In  Section  4.0  an  overview  of  TRAM  operation  and  its  applications  is  provided. 

The  discussion,  although  purely  qualitative,  illustrates  the  processing 
techniques  employed  in  TRAM  and  the  capabilities  therein  achieved. 

Section  5.0  provides  the  mathematical  development  on  which  TRAM  is  based. 

The  fundamental  estimation  and  error  analysis  equations  are  developed  in  u 

ii 

this  section.  jj 

A discussion  of  computational  techniques  and  trades  is  given  in  Section  6.0.  \ 

Included  are  specific  algorithms  and  methods  for  computer  implementation.  j 

) 

Section  7.0  is  a discussion  of  program  requirements  which  must  be  satisfied  \ 

by  TRAM.  Guidelines  for  program  development,  rather  than  detailed  specifi-  j 

cations  are  given  in  this  section.  1 

The  report  also  includes  a set  of  appendices.  In  the  main,  the  appendices  I 

provide  support  for  the  material  in  Sections  5.0  and  6.0.  However,  some  j 

discussion  of  vehicle  and  metric  instrumentation  systems,  together  with  ; 

mathematical  models,  is  also  included. 
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3.0  SYSTEM  CONSIDERATIONS  APPLIED  TO  ESTIMATOR  IMPLEMENTATION 


The  processing  which  is  implemented  in  TRAM  is  based  on  the  theory  of 
optimal  linear  estimation.  The  fundamental  assumptions  required  for 
optimality,  together  with  the  basic  algorithms  of  linear  estimation, 
are  discussed  in  Appendix  A.  In  order  to  implement  these  algorithms 
in  a manner  which  most  nearly  satisfies  the  conditions  required  for 
optimality,  it  is  advantageous  to  view  the  tracking  and  estimation 
functions  as  a composite  system. 

The  utility  of  the  system  viewpoint  is  that  it  enables  a clear  distinction 
to  be  drawn  between  tracker  and  estimator  functions.  This  in  turn  leads 
to  the  establishment  of  system  interfaces  which  facilitate  the  implementa- 
tion of  the  estimator  in  an  optimum  form. 

In  order  for  an  estimator  to  be  optimum,  its  mechanization  must  be  based 
on  models  for  all  processes  which  have  occurred  in  the  generation  of  the 
measurements  at  its  input.  Thus,  if  the  interfaces  between  the  estimator 
and  the  functions  which  precede  it  are  not  carefully  selected,  the  result- 
ing estimator  either  will  be  overly  complicated  or  it  will  perform  sub- 
optimal  ly. 

Much  of  the  discussion  in  Appendix  C is  directed  to  establishing  the  inter- 
faces between  the  tracking  and  estimation  functions  by  identifying  the 
most  useful  outputs  from  tracking  instrumentation.  These  outputs  are 
shown  to  include  data  from  both  the  encoder  and  the  sensing  element  of 
each  tracking  channel. 

In  Appendix  C.3,  the  effect  of  collecting  and  processing  only  the  encoder 
data  is  analyzed.  It  is  shown  that  this  results  both  in  suboptimum  smooth- 
ing and  in  the  introduction  of  tracking  error,  neither  of  which  can  be 
fully  compensated  in  the  estimator. 

Since  an  optimum  estimator  inherently  performs  smoothing  of  noise  process 
errors  and  compensation  for  modeled  systematic  error,  pre-processing  of 
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track  data  for  either  of  these  purposes  is  superfluous.  In  particular, 

[i  pre-smoothing  of  track  data  may  destroy  information  and  result  in  degraded 

> estimator  performance. 

\ ■ 

The  above  considerations  are  reflected  in  the  formulation  of  TRAM  processing 
algorithms.  Considerable  emphasis  has  been  placed  on  minimizing  the  intro- 
duction of  processing  error.  Also,  the  algorithms  have  been  devised  to 
allow  optimum  processing  of  joint  encoder  and  sensing  element  outputs  when- 
ever both  #re  available. 
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The  purpose  of  this  section  is  to  provide  an  overview  of  the  principal  TRAM 
operations,  and  develop  a framework  for  subsequent  mathematics  and  program 
requirements  sections.  To  illustrate  TRAM  operation  a multiple  reentry 
vehicle  (RV)  mission  will  be  considered.  A typical  mission  is  depicted  in 
Figure  4.1.  Illustrated  there  are  the  trajectory  segments  of  the  boost 
vehicle  (BV)  and  the  RVs  from  launch  to  impact.  Also  noted  are  the  separa- 
tion and  pierce  points  of  each  RV. 

During  the  mission,  off-board  data  is  collected  by  various  metric  sensor 
systems  and  on-board  data  is  collected  by  telemetry  systems. 

Post  mission  processing  of  the  metric  and  telemetry  data  is  performed  by 
TRAM.  The  objectives  of  TRAM  processing  are  twofold: 

1.  Optimal  estimation  of  selected  trajectory,  instrumentation, 
geodetic,  geopotential,  and  aerodynamic  parameters. 

2.  Error  analysis  of  the  estimated  parameters. 

4.1  Estimation 

The  parameters  to  be  estimated  are  selected  from  a state  vector  composed  of 
the  following  groups: 

1.  DYNAMIC  (TIME  VARYING)  TRAJECTORY  GROUP 

a.  position  and  velocity  for  each  vehicle 

b.  time  correlated  IMU  or  aerodynamic  parameters 
induced  by  random  phenomena 

2.  METRIC  SENSOR  GROUP 

a.  sensor  and  pedestal 

b.  beacon 

c.  refraction 

d.  geodetic 

e.  timing 
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FIGURE  4.1  MULTIPLE  RV  MISSION 
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3.  INERTIAL  MEASURING  UNIT  (IMU)  GROUP 


a.  timing 

b.  platform,  gyro,  and  accelerometer 
4.  STATIC  (CONSTANT)  TRAJECTORY  GROUP 

a.  geopotential 

b.  aerodynamic 

In  essence  the  state  vector  consists  of  all  parameters  which  appear  in 
either  trajectory  or  instrumentation  equations.  The  reasons  for  partition- 
ing and  ordering  the  state  vector  elements  into  the  above  groups  will  be- 
come clear  in  Section  6.0. 

The  subset  of  state  vector  elements  which  are  estimated  are  called  the 
estimated  states.  The  remaining  state  vector  elements  are  called  the 
constrained  states. 

4.2  Error  Analysis 

The  error  in  an  estimated  parameter  is  defined  in  general  by 
error  = estimate  - true. 

The  purpose  of  an  error  analysis  is  to  quantify,  to  the  extent  possible, 
the  errors  which  remain  after  estimation.  Since  the  true  values  of  the 
parameters  are  not  generally  known  (except  in  simulations)  the  best  that 
can  be  done  is  to  provide  a probabilistic  description  of  estimation  error. 

An  example  of  such  a description  would  be  the  means,  variances,  and  cross 
correlations  of  the  set  of  estimation  errors. 

An  estimation  error  analysis  is  a two  stage  process.  The  first  stage,  which 
is  performed  concurrently  with  estimation,  consists  of  calculating  the 
sensitivities  of  the  parameter  estimates  to  each  of  the  error  sources  to  be 
considered.  The  second  stage  combines  these  sensitivities  with  an  error 
budget  (i.e.,  a statistical  description  of  the  errors  for  the  sources  being 
considered)  to  obtain  a probabilistic  characterization  of  the  estimation 
errors.  The  second  stage  can  either  be  performed  conciirrent  with  or  sub- 
sequent to  estimation. 
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The  sources  of  error  considered  in  the  error  analysis  generally  include  sensor 
and  trajectory  noise  errors  and  state  vector  initialization  errors. 

The  subset  6f  states  whose  initialization  errors  are  included  in  the  error 
analysis  are  called  propagated  states.  The  set  of  propagated  states,  which 
always  includes  the  estimated  states,  may,  in  addition.  Include  any  subset 
of  the  constrained  states. 

4.3  Sequential  Linear  Estimation 

In  general,  an  optimal  linear  estimator  can  be  Implemented  by  means  of  a 
two  stage  sequential  algorithm.  The  two  stages  are,  respectively,  filter 
and  smoother.  The  application  of  a sequential  two  stage  linear  estimator 
to  the  multiple  RV  mission  is  illustrated  in  Figure  4.2.  Examination  of 
the  flow  diagram  shown  in  this  figure  reveals  that  all  trajectory  segments 
for  boost  and  reentry  vehicles  are  first  filtered,  and  then  each  segment  is 
smoothed.  The  order  in  which  the  segments  are  filtered  is  somewhat  arbitrary, 
but  the  order  of  smoothing  is  the  exact  reverse  of  the  filtering  order. 
Furthermore,  the  filter  operates  on  each  segment  by  processing  the  data  in 
the  direction  of  increasing  time.  The  smoother,  on  the  other  hand,  processes 
the  filter  outputs  in  the  direction  of  decreasing  time. 

The  filter  provides  estimates  utilizing  only  the  data  processed  up  to  and 
including  estimation  time.  The  smoother  provides  estimates  utilizing  all 
of  the  data,  by  adjusting  the  filter  estimates.  It  should  be  noted,  however, 
that  only  the  dynamic  parameter  estimates  (i.e. , those  in  Group  1)  require 
adjustment  by  smoothing.  The  estimates  of  the  static  (constant)  parameters 
which  are  obtained  at  the  end  of  the  filter  stage  are  unaffected  by  smoothing, 
since  these  estimates  are  already  based  on  the  entire  data  set. 

For  processing  convenience  and  flexibility,  each  trajectory  segment  can  be 
partitioned  into  regions  of  powered  flight,  f reef all,  and  reentry  and  these 
regions  can  be  further  partitioned  into  Intervals  over  which  sensor  coverage 
does  not  change. 

In  addition,  in  order  to  facilitate  and  enhance  the  efficiency  of  bulk 
storage  (i.e.,  disk  or  tape)  Input/output  (I/O)  operations,  the  Intervals 
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FIGURE  4.2  TWO  STAGE  OPTIMAL  LINEAR  ESTIMATOR  APPLIED 
TO  MULTIPLE  RV  MISSION 
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mentioned  above  can  be  further  partitioned  into  subintervals  at  tne  junc- 
tures of  which  all  I/O  operations  are  performed.  The  size  of  these  sub- 


intervals can  be  selected  small  enough  such  that  the  instantaneous  storage 
capacity  in  the  computer  memory  is  not  exceeded  and  large  enough  to  maintain 
the  number  of  I/O  operations  sufficiently  small  that  computer  efficiency 
does  not  suffer. 

During  filtering,  the  input  operations  entail  reading  metric  and  telemetry 
data,  and  the  output  operations  consist  of  writing  filter  estimates  of  the 
whole  state  and  the  sensitivities  of  these  estimates  to  the  propagated  states. 
During  smoothing,  the  input  operations  read  filter  outputs,  ard  the  output 
operations  write  smoother  estimates  and  the  sensitivities  of  these  estimates 
to  the  propagated  states. 

4.4  Nonlinear  Sequential  Estimation 

In  order  to  apply  linear  estimation  to  the  multiple  RV  mission,  a further 
refinement  is  necessary,  because  the  equations  governing  such  missions 
are,  in  fact,  nonlinear.  The  refinement  to  be  discussed  is  analogous 
to  the  use  of  the  Newton-Raphson  method  for  solving  nonlinear  algebraic 
equations.  The  algebraic  method  uses  relinearization  and  iteration  to 
obtain  a solution.  On  each  Iteration  a set  of  linear  equations  is  solved, 
and  the  solution  is  used  to  relinearize  the  nonlinear  equations  to  obtain 
the  set  of  linear  equations  to  be  solved  on  the  next  iteration.  The  pro- 
cess is  terminated  when  convergence  occurs,  i.e. , when  identical  solutions 
are  obtained  on  successive  iterations. 


In  the  estimation  problem,  the  nonlinear  equations  are  linearized  about  a 
nomi nal  value  of  the  state  vector  at  each  step  in  the  sequential  process. 
The  nominal  value  of  the  state  vector  is  an  arbitrary  approximation  to  the 
true  state  vector.  The  resulting  equations  are  linear  in  the  state  vector 
variation  which  is  defined  component  by  component  by 

variation  = true  - nominal. 

The  linear  estimation  equations  are  applied  to  estimate  the  variation.  The 
estimate  of  the  whole  state  vector  Is  then  given  component  by  component  by 

whole  estimate  = nominal  + variation  estimate. 
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For  each  component  in  the  subset  of  constrained  states,  the  whole  estimate 
is  by  definition  equal  to  the  constrained  value  of  the  component. 

The  technique  for  nonlinear  estimation  consists  of  iterating  the  entire 
two  stage  sequential  filter/smoother  operations.  On  each  iteration,  the 
variation  estimate  obtained  upon  completion  of  the  filtering  and  smoothing 
of  operations  is  added  to  the  nominal  to  obtain  an  estimate  of  the  whole 
state  vector.  The  whole  state  estimate  thus  obtained  is  then  used  as  the 
nominal  state  vector  on  the  next  iteration.  This  relinearization  and 
iteration  process  continues  until  the  variation  estimates  converge  to  zero, 
i.e.,  the  whole  state  estimates  on  two  successive  iterations  are  identical. 

On  the  first  pass  the  nominal  state  vector  is  initialized  with  the  best 
prior  estimate  available.  Then  during  the  filter  stage  the  nominal  state 
is  reset  periodically  by  equating  the  nominal  state  to  the  whole  value 
estimate  obtained  by  the  filter. 

The  questions  regarding  convergence  of  the  above  procedure  are  not  easily 
answered.  While  there  are  well  known  necessary  and  sufficient  conditions 
for  convergence,  they  are  not  easily  verified  for  the  class  of  trajectory 
estimation  problems  of  interest  here.  However,  experience  has  shown  that, 
in  well  formulated  problems  of  this  nature,  convergence  generally  occurs 
within  several  iterations  provided  the  nominal  state  is  initialized  suf- 
ficiently close  to  the  true  state.  The  periodic  nominal  reset  procedure 
described  above  is  designed  to  maintain  the  variation  sufficiently  small  on 
the  first  pass  that  convergence  occurs  quickly  thereafter. 

4.5  Recapitulation 

The  estimation  procedure  and  the  error  analysis  which  can  be  performed 
concurrently  with  estimation  are  concisely  represented  by  the  flow  diagram 
of  Figure  4.3.  The  diagram  illustrates  an  outer  loop  for  relinearization 
and  iteration,  middle  loops  for  interval  processing,  and  inner  loops  for 
processing  of  subintervals.  Counters  I,  J,  and  K control  the  outer,  middle 
and  inner  loops,  respectively. 
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The  outer  loop  Is  repeated  until  convergence  is  achieved  or  the  maximum 
allowed  number  of  iterations  has  occurred.  The  middle  and  inner  loops  are 
traversed  In  a forward  direction  (counters  increasing)  during  the  filter 
stage  and  In  the  reverse  direction  (counters  decreasing)  during  the  smoother 
stage.  Although  it  is  not  shown  in  the  diagram,  within  each  subinterval 
the  filter  processes  input  data  sequentially  with  time  increasing,  and  the 
smoother  processes  filter  data  sequentially  in  the  reverse  (i.e.,  time 
decreasing)  direction. 

Each  interval,  it  will  be  recalled,  consists  of  a trajectory  segment  portion, 
which  is  exclusively  powered  flight,  freefall,  or  reentry,  over  which  sensor 
coverage  does  not  change.  The  interval  discrete  functions  control  filter/ 
smoother  configuration  over  each  interval  by  specifying  sensors  to  be 
processed,  parameters  to  be  estimated,  times  of  discrete  events  within  the 
estimator,  and  so  forth. 

The  subinterval  discrete  functions  consist  entirely  of  bulk  storage  I/O 
operatl ons . 

It  was  stated  earlier  that  the  error  analysis  and  estimation  procedures  can 
be  performed  concurrently.  This  is  accomplished  by  augmenting  the  filter 
and  smoother  with  error  propagation  functions.  However,  because  of  the 
iterative  feature  of  the  estimator,  it  is  more  efficient  to  allow  the 
estimator  to  converge  before  performing  the  error  propagation  functions. 

Thus,  with  reference  to  Figure  4.3,  after  convergence,  one  more  pass  through 
the  outer  loop  is  taken  in  which  only  the  error  propagation  functions  are 
performed. 


5.0 


MATHEMATICAL  DEVELOPMENT 


In  Section  4.0,  the  major  TRAM  functions  of  estimation  and  error  analysis 
were  discussed  in  qualitative  terms  for  a multiple  RV  mission.  The  concept 
of  a state  vector  was  introduced,  and  its  composition  by  various  groups  was 
presented.  The  state  vector  consists  of  parameters  which  affect  either 
trajectories  or  measurements.  For  purposes  of  estimation,  two  subvectors 
of  the  state  vector  are  defined.  The  first  contains  the  estimated  states. 
The  second  contains  the  constrained  states.  For  the  error  analysis,  a sub- 
vector of  propagated  states  is  defined  which  contains  all  the  estimated 
states  and,  in  addition,  some  subset  of  the  constrained  states. 

In  this  section,  the  basic  equations  for  both  estimation  and  error  analysis 
will  be  developed  using  the  state  vector  concept. 

The  state  vector,  denoted  by  x,  satisfies  a nonlinear  differential  equation 

(1)  x(t)  « f[x_(t),  t]  + w(t) , t > tQ  , 

called  the  state  equation.  The  metric  sensor  measurements,  denoted  by  y, 
are  functions  of  state  at  discrete  times  and  satisfy  a nonlinear  equation, 

(2)  y(ti)  = hfx( t . ) , t.]  + v^),  i * 0,  1,  2 

called  the  measurement  equation.  The  quantities  w and  v are  called  the 
state  (or  plant)  and  measurement  noise  processes,  respectively. 

While  the  TMIG  measurements  can  also  be  expressed  in  the  form  given  by  (2), 
it  is  more  convenient  to  express  these  measurements  in  a form  suitable  for 
introduction  in  the  right  hand  side  of  (1)  as  a direct  measurement  of 
vehicle  dynamics. 

If  the  state  vector  is  ordered  and  partitioned  into  the  four  groups  defined 
in  Section  4.0,  equation  (1)  becomes 


where  the  groups  are  Identified  as  follows: 


xa,  dynamic  trajectory  group 

a 

Xfj,  metric  sensor  group 
x , inertial  sensor  group 

v 

x^,  static  trajectory  group 
Similarly,  equation  (2)  becomes 

(4)  yUj)  = h[xa(ti),  xb,  t13  + v(t.|),  i » 0,  1,  2 

Equation  (3)  shows  explicitly  the  dynamic  behavior  of  the  first  group  and 
the  static  nature  of  the  remaining  groups.  Also  shown  is  the  fact  that  In 
general  the  dynamic  behavior  of  xfl  depends  on  xfl,  xc  and  x^.  However,  the 
dependence  of  x,  on  x.  occurs  only  In  inertially  instrumented  trajectory 
spans.  Equation  (4)  shows  that  the  metric  sensor  measurements  depend 
only  on  x#  and  xb. 

The  state  noise  component  wa(t)  represents  the  effect  of  random  forces, 

a 

such  as  aerodynamic  forces,  on  those  trajectory  spans  which  are  not 
inertially  instrumented.  On  inertially  instrumented  spans,  all  inertial 
forces,  including  random  forces,  are  sensed  and  Incorporated  into  fa;  and 

a 

w (t)  represents  noise  In  the  Inertial  instruments. 

3 

5.1  Estimation 

In  this  subsection,  equations  (1)  and  (2)  will  be  used  as  a point  of 
departure.  However,  explicit  dependence  on  the  subvector  of  constrained 
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states  will  be  suppressed,  and  x will  denote  only  the  subvector  of  estimated 
states. 


To  apply  linear  estimation  techniques,  equations  (1)  and  (2)  must  be  linearized 
with  respect  to  the  state  vector.  This  can  be  accomplished  over  any  time 
interval  of  interest,  say  [t* , t"],  in  the  following  manner. 


Let  x denote  the  solution  of  the  differential  equation 

(5)  x(t)  = f[x(t),  t],  t'  < t < t"  , 


with  x(t')  arbitrarily  specified,  x is  called  the  nominal  state  vector. 
Then,  expansion  of  equations  (1)  and  (2)  to  first  order  about  x leads  to 

(6)  x ( t)  = f[x(t),  t]  + F(t)[x(t)  - x(t)]  + w(t)  , 

(7)  y(t1 ) - hlX^),  t^]  + H(ti)[x(ti)  - x(tj)]  + v^)  , 


where 

(8)  F(t)  = 
and 

(9) 

Now  define  the  state  and  measurement  variations,  respectively,  by 


yax  f x(t),  t 


8(ti ) E (^tV/ 

\ax  / x^.),  ti 


(10)  6x(t)  = x(t)  - x(t)  , 
and 

(11)  6y(tt)  = y( t^ ) - hExftj),  tf]  . 
From  (5),  (6),  and  (7)  it  then  follows  that 


1 


(12) 


<5x(t)  = F(t)<5x(t)  + w(t)  , 


and 


(13)  fiyt^)  = H(t^  )6x(t1)  + vt^) 


for  all  t and  in  the  interval  [t‘,  t"]. 


The  solution  of  (12)  can  be  expressed  in  a convenient  form  using  the 
transition  matrix,  4>,  defined  by 


(14)  $(t,  s)  * 'F(t)'F“1  (s) 


for  all  t and  s in  [t‘,  t"],  where  V is  the  so-called  fundamental  matrix 
defined  by  the  differential  equation 


(15)  f(t)  = F(t)y(t),  y(t')  = I,  t’  < t < t"  . 


The  existence  of  (t)  is  guaranteed  for  all  t'  < t < t",  and  thus  <t  is  well 


defined  by  (14). 


In  terms  of  $,  the  solution  of  (12)  is  expressed  by 

(16)  6x(t)  = $(t,  s)5x(s)  + J 4>(t,  r)w(r)dr 


for  all  t and  s in  [t',  t"].  In  particular,  for  t^  and  t^-j  in  [t',  t"], 


(17)  Mt1+1)  = ^(t1+1 , t^Gxftj)  + u(t^) 


where 


/ti+l 

3>(ti+1,  r)w(r)dr  . 


ui 


By  specifying  x at  a finite  set  of  times  and  using  (5)  to  obtain  x in  the 
intervals  between  these  times,  the  linear  equations  obtained  above  can  be 


i i 

i'l 
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extended  to  hold  for  all  times  of  Interest  on  the  multiple  vehicle  trajectories. 
Thus  the  linear  state  and  measurement  equations  become 


(19)  <5x(t.j+i ) = ♦(t^'j,  t^  )<5x(t.j ) + u(ti)  , 

(20)  6y(ti)  = H(t.)6x(t.)  + v(t.),  1 * 0,  1,  2,  .... 

with  the  provision  that  whenever  x is  reset,  6x  must  also  be  reset  by  the 
relation 

(21)  5x(t+)  = 6x(t")  - [x(t+)  - x(t")]  , 

where  t~  and  t+  denote  values  before  and  after  reset,  respectively. 

Equations  (19)  and  (20)  provide  the  model  basis  for  application  of  the  two 
stage  (filter/smoother)  linear  estimation  procedure  which  will  now  be 
developed.  With  somewhat  briefer  notation,  the  estimation  model  is  given 
by 

(22)  Sxi+1  * *.6x.  + ui  , 

(23)  6y.j  = Hi6xi  + v.,  i =0,  1,  2 

The  sequential  filtering  and  smoothing  algorithms  of  linear  estimation, 
together  with  the  assumptions  on  which  they  are  based,  are  discussed  fully 
in  Appendix  A. 

The  filter  algorithm  for  (22)  and  (23)  is  given  by 

(24)  K.  = PTH^PTH}  + R.]'1  , 

(25)  6x.  = Sx~  + K.(6y.  - H.6xT)  , 

(26)  P,  - PT  - ^H.P"  , 

(27)  6Xi+-j  = 4j6x^  , 
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(28) 


^■{+1  = Q-j » ^ * 0»  1*  2,  .... 

A 

Qi  and  Ri  are  the  covariances  of  ui  and  v^,  respectively.  6xj  is  the  estimate 

A 

of  6xi  basjad  on  the  set  {yQ y^h  6xj  1s  the  estimate  of  5x1 

based  on  the  set  (yQ,  ....  y^}.  P^  and  are  the  respective  filter  covariances 

A A 

of  6xT  and  Sx^ . K..  is  the  filter  gain  matrix. 

A 

The  estimates  6x  and  the  filter  covariances  P are  computed  sequentially.  At 

A 

each  step  5xT,  based  on  measurements  {yQ,  ....  y.^},  is  updated  using  y.  to 
A A 
obtain  6x..,  and  P.  is  updated  to  obtain  P..  Then  6x.  is  extrapolated  to 

A 

obtain  6xT+1,  based  on  measurements  {yQ,  ....  y^},  and  Pi  is  extrapolated 
to  obtain  P^+1. 

The  smoother  algorithm,  to  be  considered  here,  has  two  forms,  one  of 
which  can  only  be  used  in  the  special  case  in  which  there  is  no  state 
noise.  In  either  case  the  smoother  is  employed  after  the  complete 
measurement  set  {yQ,  ....  yN)  has  been  filtered. 

The  most  general  form  of  smoother,  to  be  used  here,  is  the  fixed  interval 
smoother,  and  the  algorithm  for  this  form  is  given  by 


(29) 

(30) 

(31) 


Ai  = Pi$i^Pi+l^  1 ’ 


A A 

i|N  = xi  + Vxi+1|N  ' xi+l ^ ’ 

Pi  " Ai (pi+l  ' Pi+l|N^Ai’  1 = N_1’  0 • 


Pi  |N 


is  the  smoother  gain  matrix,  x^^  is  the  estimate  of  x^  based  on  the 
complete  measurement  set  {yQ,  ...,  yN>,  and  P^N  is  the  smoother  covariance 
of  . 


The  fixed  interval  smoother  is  Initialized  by  the  final  values  of  the  filter. 
That  is 


1 


and 

PN | N = PN  * 

The  smoother  quantities  are  computed  sequentially  but  In  reverse  order  to 

A A 

the  filter.  At  each  step  x^|N  is  obtained  from  x^+^|N  and  filter  outputs. 
Similarly  is  obtained  from  and  filter  outputs. 

The  filter  estimates  of  the  whole  state,  required  In  the  smoother,  are 
obtained  during  the  filter  operation  by  simply  computing  and  storing 
the  quantities 

(32)  Xj  = 6x.j  + x,j  , 
and 

(33)  X-  = <Sx.j  ^ Xj , l s 0,  1,  ...,  N • 

It  should  be  noted  that,  unlike  the  filter  covariances,  the  smoother 
covariances  are  not  required  to  obtain  the  smoothed  estimates  of  the  state 
vector.  However,  the  smoother  covariances  are  required  for  the  error 
analysis.  In  fact,  if  all  the  conditions  stated  in  Appendix  A were 
satisfied,  the  smoother  covariance  would  be  equal  to  the  covariance  of 
estimation  error. 

The  second  form  of  the  smoother  which  can  be  used  only  when  state  noise  is 
zero  is  the  retrograde  intergatlon  smoother.  This  method  of  solution  consists 
of  simply  solving  equation  (1)  (where  w(t)  = 0)  by  reverse  or  retrograde 
integration  from  t^  to  tQ  using  the  final  value  estimate  from  the  filter  to 
initialize  the  integrator.  The  smoother  covariance  is  similarly  obtained 
by  retrograde  integration  using  the  final  value  of  the  filter  covariance 
for  Initialization. 


i 1 i 
I J 

.j 

j 

j 
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The  retrograde  Integration  algorithm  is  given  by 


(34)  *(t|tN)  = f[x(t|tN),  t],  x(tN|tN)  - xN  , 

(35)  P(t|tN)  = F(t)tN)P(t|tN)  + P(t|tN)FT(t|tN),  P(tN|tN)  = PN,  tQ  < t < tN, 
where 

(36)  Xjj  = <5x^  + x(tN)  , 

(37)  F(t|tN)  * ~ 

n \3x  / x(t|tN),  t 

A 

x(t|tN)  is  the  whole  value  state  estimate  based  on  {yQ,  . ...  yN>,  and 

/v 

P(t|tN)  is  the  smoother  covariance  of  x(t|t^). 


A flow  diagram  for  the  two  stage  estimator  illustrating  the  basic  functions 
of  the  filter  and  both  smoother  types  is  illustrated  in  Figure  5.1.  The 
diagram  is  somewhat  simplified,  but  serves  the  purpose  of  demonstrating  the 
essential  structure  of  the  estimator. 

There  are  two  major  filter  functions,  update  and  extrapolate,  which  are 
prepared  by  measurement  processing  and  Integration  functions,  respectively. 
Notice  als6,  that  In  the  filter  stage  the  variation  estimate  must  be  reset 
whenever  nominal  reset  occurs  (Cf.  equation  (21)). 

5.2  Error  Analysis 

The  estimation  equations  which  have  been  developed  in  the  preceding  subsection 
deal  exclusively  with  the  set  of  estimated  states.  In  the  linearization 
procedure,  it  was  tacitly  assumed  that  the  variation  (i.e.,  true  - nominal) 
was  zero  for  each  constrained  state.  Since  this  subsection  deals  with  all 
error  sources.  Including  constrained  states,  nonzero  variations  of  these 
states  will  be  considered. 


E C 

Let  x denote  the  subvector  of  estimated  states,  and  let  x denote  the 

subvector  of  constrained*  states  to  be  included  in  the  error  analysis. 


Then  repetition  of  the  linearization  procedure  used  In  the  preceding 
subsection  leads  to  analogous  equations  which  Include  the  variations  In 
the  constrained  states.  The  analogues  to  equations  (12)  and  (13)  are 


(38) 

(39) 
where 

(40) 


SxE(t)  * F(t)6xE(t)  + G(t)6x£  + w(t)  , 
MV  - H(ti)5xE(t.)  + JfVfixJ;  + v(ti) 


(3{xo)T)x(t),  t 
' (#)x(V,  ti 


3 (t1 ) = 


The  analogues  of  equations  (22)  and  (23)  are 


(42) 

6x1+l 

s $^6x^  + u^  * 

(43) 

6y1  * 

H.6xE  + Ji6xo  + v1*  1 = °»  2»  •••» 

where 

(44) 

e.  = 

>*1+1 

J *(ti+],  r)S(r)dr  . 

Now  equations  (42)  and  (43)  can  be  used  to  develop  the  error  propagation 
equations  for  the  filter  and  smoother.  The  filter  errors  are  defined  by 

(45)  ej  = 6xE  - 6xE  , 

(46)  e1  = 6xE  - 6xE,  1 = 0,  1,  2 N . 
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( 


The  fixed  interval  smoother  error  Is  defined  by 


(47)  ®i]N  8 ^1 1 N ” ^1*  ^ = ^ •••»  N , 


and  the  retrograde  smoother  error  Is  defined  by 

(48)  e(t|tN)  * xE(t|tN)  - xE(t),  tQ  < t < tN  . 

From  the  error  equations  and  the  filter  algorithm,  it  follows  that 


(49)  e.  = (I  - K^.JeT  + ^J^xj;  + , 


ei+]  = Vi  • ®18,o  - U1 


Similarly  for  the  two  smoother  types 


ei |N  = ei  + Aitei+1|N  " ei+l^  * 


e(t|tN)  * F(t|tN)e(t|tN)  - G(t|tN)6Xp  , 


where 


F(t|tJ  = 


(a(xE)T)  x(t|tN),  t 


v ' (w);(t, o..‘ 


The  estimation  error  at  any  point  in  the  estimation  process  is  a linear 
combination  of  initialization  errors  in  the  estimated  and  constrained  states 
and  errors  due  to  measurement  and  state  noise.  Define 


3<*o> 
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Then  the  Initialization  error  propagation  equations  are 


(55) 

°1  " 

(i  - o;  - I . 

(56) 

E1  = 

(I  - + K^,  EjJ  = 0 , 

(57) 

°H1  = 

*i°i  * 

(58) 

Ei+1  " 

*1E1  " ®1  » 

(59) 

°i|N  = 

Di  + Ai  ^-D-|+l  j N " D1+l  ] * DN | N = °N 

(60) 

E1|N  = 

Ei  + Ai^Ei+l |N  ‘ Ei+1^*  EN|N  = EN 

(61) 

0(t|tN) 

* F(t|tN)D(t| tN),  D(tN|tN)  = Dn  , 

(62) 

E(t|tN) 

= F(t|tN)E(t|tN)  - G(t(tN),  E(tN|tN) 

A procedure  for  the  error  analysis  using  the  above  error  propagation  equations 
together  with  the  filter  and  smoother  error  covariance  equations  will  now  be 
developed.  The  error  analysis  will  provide  the  mean  and  covariance  of  the 
estimation  error  based  on  an  error  budget. 

The  error  budget  specifies  the  mean  and  covariance  of  initialization  error: 


kEl 

[uEE 

UEC‘ 

0 

• 

0 

0 

bj 

UEE 

yce 

0 

w J 

0 

0 

It  also  specifies  the  state  and  measurement  noise  process  covariances 
Q^»  i * 0,  1,  2,  ...,  and  Rj,  1 =0,  1,  2,  ....  and  It  is  assumed  that 
these  noise  processes  are  sequentially  uncorrelated  and  mutually  Independent 
with  each  other  and  the  initial  value  of  the  state. 


i 
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The  mean  or  bias  estimation  error  Is  given  by 


b1 |N  = °1 | Nbo  + E1 | Nbo  ’ 


In  the  fixed  Interval  smoother,  and  by 
(64)  b(t|tN)  = D(t|tN)bE  + E(t|tN)bJ 


In  the  retrograde  Integration  smoother. 

The  covariance  of  the  estimation  error  due  to  Initialization  error  is  given 

by 


= [di|n  i ei|n] 


uCE  ucc 
0 0 


in  the  fixed  interval  smoother,  and  by 


(66)  U(t|tN)  = [D(t|tN)  ! E(t|tN)] 


in  the  retrograde  integration  smoother. 


«p 

o o 
UCE  UCC 


DT(t|ty) 

ET(t|tN) 


The  covariance  of  estimation  error  due  to  the  noise  processes  is  most  easily 
computed  by  an  indirect  method  using  the  smoother  covariance.  The  method  is 
valid  only  if  the  error  budget  values  of  the  noise  process  covariances, 

Q.j»  i =0,  1,  2,  ....  and  R^,  i =0,  1,  2 are  used  in  the  filter 

algorithm  (Cf.  equations  (24)  and  (28)).  Under  this  assumption,  the  esti- 
mation error  covariance  due  to  noise  processes  is  given  by 

(67)  = Pi|N  - Di J nPoDT ( N ’ 


(68)  V(t|tN)  = P(t|tN)  - D(t|tN)P“D'(t|tN)  , 
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where  is  the  Initial  covariance  used  in  the  filter. 

In  the  special  case  in  which  state  noise  is  zero  and  the  retrograde  integration 
smoother  Is  used,  an  alternate  direct  method  of  calculating  the  estimation 
error  covariance  due  to  measurement  noise  may  be  used.  This  method  Is  valid 
regardless  of  whether  the  error  budget  values  of  measurement  noise  are  used 
in  the  filter.  The  first  step  is  to  compute  and  by 

(69)  Vf  - (I  - K^V'U  - K1Hi)T  + K^Kj.  V;  = 0 , 

(7°)  v;+1  = 1 = 0,  1,  2,  ... 

Then  the  estimation  error  covariance  due  to  measurement  noise  is  given  by 

(71)  VUiy  = F(t|tN)V(t|tN)  + V(t|tN)  FT(t|tN),  V(tN|tN)  » VN  . 

Finally,  the  total  estimation  error  covariance  due  to  combined  initialization 
and  noise  process  error  is  given  by  either 

(72)  M1(n  - U1)N  + Vi)N,  1 • 0,  1,  2,  .... 
or 

(73)  W(t | t^)  = U(t|tN)  + V(t|tN),  t0  < t < tN  . 

A flow  diagram  for  the  computation  of  the  error  propagation  quantities  D,  E, 
and  V Is  presented  In  Figure  5.2.  The  alternative  calculation  of  V is  shown 
as  an  option  to  be  exercised  only  when  state  noise  is  zero  and  the  measurement 
noise  error  budget  values  differ  from  those  used  in  the  filter  algorithm. 

The  sequential  nature  of  the  error  propagation  equations  is  in  one  to  one 
correspondence  with  the  filter  and  smoother  equations.  Consequently,  the 
error  propagation  equations  could  be  easily  computed  in  parallel  with  the 
estimation  equations.  However,  because  of  the  iterative  process  used  to 
minimize  error  due  to  nonlinearity,  it  is  most  efficient  to  defer  the 
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calculation  of  the  error  propagation  equations  until  the  estimation  process 
converges. 


Once  the  error  propagation  quantities  have  been  calculated  and  the  Initiali- 
zation error  budget  has  been  specified,  the  error  analysis  Is  completed  using 
equations  (63)  or  (64)  to  compute  the  estimation  bias  error,  and  (65)  and 
(72)  or  (66)  and  (73)  to  compute  the  covariance  of  estimation  error. 


6.0 


COMPUTATIONAL  CONSIDERATIONS 


In  order  to  effectively  select  and  implement  an  algorithm  in  a computer 
program,  due  consideration  of  several  factors  is  required.  These  factors 
include  the  numerical  precision  of  the  computer  and  the  accuracy,  stability, 
and  efficiency  of  the  algorithm. 

The  numerical  precision  of  a computer  Is  determined  by  the  number  of  digits 
allocated  to  the  mantissae'of  numbers  rep> ~ rented  in  the  machine.  The  error 
which  occurs  because  of  limited  numerical  precision  is  called  roundoff  error. 

All  other  computational  errors  are  inherently  due  to  the  algorithm  itself. 

The  most  common  of  these  is  truncation  error.  Truncation  error  results  when 
higher  order  terms  in  a Taylor  series  expansion  are  neglected. 

The  stability  of  the  algorithm  is  determined  by  how  roundoff  and  truncation 
errors  propagate.  If  these  errors  propagate  in  an  unbounded  or  oscillatory 
manner  the  algorithm  is  unstable. 


Efficiency  of  an  algorithm  is  a relative  concept  based  on  execution  time 
and  storage  requirements.  Gross  estimates  of  execution  time  and  storage 
requirements  must  be  considered  in  selecting  candidate  algorithms,  and 
relative  efficiency  can  be  used  as  a tradeoff  basis  for  otherwise  comparable 
algorithms. 


In  the  remainder  of  this  section,  the  computer  implementation  necessary  to 
satisfy  the  functional  requirements,  developed  in  Section  5.0,  for  estimation 
and  error  analysis  will-be  discussed  based  on  consideration  of  the  compu- 
tational factors  mentioned  above. 


6.1 


Scalar  Measurement  Update 


The  update  equations  of  the  filter  algorithm  developed  in  Section  5.0  are 
given  by 


K = P-HT[HP-HT  + R]"1  , 
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<$x  = 6x"  + K(Sy  - H6x‘)  , 

P = P"  - KHP~ 

where  the  time  index  i has  been  dropped.  The  function  of  these  equations 

A 

is  to  process  the  measurement  variation  6y  and  thereby  update  <Sx  and  P"  to 

A 

obtain  6x  and  P respectively.  As  written  these  equations  are  valid  for  a 
measurement  vector  of  arbitrary  diminsion.  For  example,  if  6y  is  an  m-vector 
and  P is  nxn,  H-is  mxn,  R is  mxm,  and  K is  nxm. 

The  update  equations  are  based  in  part  on  the  assumption  that  the  measurement 
variation  is  given  by 

<5y  = H6x  + v , 

where  the  covariance  of  v is  the  matrix  R. 


In  the  particular  case  where  R is  diagonal*,  the  vector  update  procedure 
can  be  replaced  with  an  entirely  equivalent  scalar  update  procedure  which 
is  more  efficient. 


* ■ 

» — 

6yl 

H1 ' 

6y  = 

sy2 

, H = 

h2. 

6y 

m 

H . 
m 

, and  R 


Thus  for  each  j » 1,  ....  m,  Sy  is  the  j-th  scalar  element  of  6y,  H^..  is  the 
j-th  row  of  H,  and  R..  is  the  j-th  diagonal  element  of  R.  Now  the  scalar 

J J 

update  procedure  consists  simply  of  applying  the  update  equations  sequentially 
to  the  elements  of  6y  as  illustrated  in  Figure  6.1. 


*If  R is,  in  fact,  the  covariance  matrix  of  the  measurement  noise  vector  v, 
then  R is  diagonal  if  and  only  if  the  elements  of  v are  mutually  uncorrelated. 
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In  the  figure,  dummy  variables  6£  and  II  Mere  used  for  Immediate  results 
which  need  not  be  saved.  The  final  values  of  these  variables  yield  the 
same  values  of  6x^  and  that  would  have  been  obtained  by  the  vector  update 
procedure. 

The  gain  matrices  k|^,  j = 1,  ...»  m,  must  all  be  saved  for  use  In  the 
error  propagation  equations.  Furthermore,  equations  (55),  (56),  and  (69) 

In  Section  5.0  must  also  be  processed  sequentially  for  each  fixed  1 at  which 
the  scalar  update  method  Is  employed.  The  reason  for  this  Is  that 

Kj  t [kJ])  ! ...  ! Kjra)]  . 

I 1 I I I 

The  sequential  error  propagation  equations  are  illustrated  in  Figure  6.2. 

Dummy  variables  are  once  again  use*  for  intermediate  results  which  need  not 
be  saved. 

6.2  Square  Root  Filter 

The  standard  filter  algorithm  which  was  developed  In  Section  5.0  includes 
sequential  calculation  of  the  error  covariance  matrix,  P.  A necessary  j j 

condition  for  an  nxn  matrix  P to  be  a covariance  matrix  is  that  it  be 
l nonnegative,  l.e.,  PT  * P and 

\ T 

a Pa  > 0 

for  all  n-vectors  a.  However,  careless  computation  of  P in  the  standard 
filter  can  result  in  violation  of  the  nonnegativity  condition  because  of 
roundoff  error.  When  this  occurs,  filter  instability  can  result. 

There  are  a number  of  methods  typically  employed  to  preserve  the  nonnegativity 
of  P In  filter  implementations.  However,  some  of  these  methods  increase 
execution  time  by  as  much  as  100%.  Worse,  they  sometimes  fail  to  preserve 
nonnegativity,  expecially  If  P becomes  Ill-conditioned,  l.e.,  nearly  singular. 

An  alternative  to  employing  any  of  these  methods  Is  provided  by  the  square 
root  filter. 


The  square  root  filter,  which  can  be  realized  by  any  of  several  algorithms, 
is  mathematically  equivalent  to  the  standard  filter.  The  square  root  filter 
requires  sequential  calculation,  not  of  P,  but  a square  root  of  P,  designated 
here  by  S. 


An  nxn  matrix  S which  satisfies  the  condition 
P = SST 

is  called  a square  root  of  P.  If  P is  nonnegative,  then  a real  valued  square 
root  of  P exists,  but  it  is  not  unique.  Conversely,  for  any  real  valued 
nxn  matrix  S,  the  nxn  matrix  product  SST  is  nonnegative,  since 

aT(SST)a  = (STa)T(STa)  > 0 

for  all  n-vectors  a.  This  property  is  important  in  square  root  filtering, 
because  it  guarantees  the  nonnegativity  of  the  filter  error  covariance  matrix  P. 

Square  root  filters  have  two  distinct  advantages.  First  they  are  much  less 
sensitive  to  roundoff  error  than  standard  filters.  In  fact  empirical  computer 
studies  have  demonstrated  in  some  instances  that  square  root  filters  have 
single  precision  accuracy  that  can  be  achieved  with  a standard  filter  only  by 
using  double  precision  arithmetic.  Second,  and  most  important,  filter 
instability  due  to  .violation  of  the  nonnegativity  condition  cannot  occur, 
since  the  error  covariance  matrix  P (which  need  not  be  computed  but  which  is 
implicitly  defined  by  the  square  root  matrix  S)  is  guaranteed  to  be 
nonnegative. 

Of  the  available  square  root  filter  algorithms,  the  one  selected  for  application 
in  TRAM  is  due  to  Carlson.  This  algorithm,  which  is  given  in  detail  in  Appendix 
A,  is  very  efficient.  The  required  execution  time  is  comparable  to  the  most 
efficient  standard  filter  algorithm. 

The  Carlson  algorithm  has  the  distinctive  feature  that  the  square  root  matrix 
S is  maintained  in  triangular  form.  S may  be  either  upper  triangular  or  lower 
triangular  at  the  option  of  the  user.  Recall  that  a matrix  is  upper  (lower) 


. — j i in  i ii  i tii  ■ munr  nr  -.trriii~n> : - 
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triangular  If  all  elements  below  (above)  the  main  diagonal  are  zero.  The 
Importance  of  triangularity  Is  that  it  reduces  the  computational  and  storage 
requirements  of  S by  nearly  50%. 

The  Carlson  algorithm  parallels  the  sequential  operations  of  the  standard 
filter,  except  that  it  propagates  S instead  of  P. 

The  matrix  S is  initialized  by  extracting  a triangular  square  root  of  P~ 
using  the  Cholseky  decomposition  algorithm  given  in  Appendix  D. 

The  update  operation  is  constrained  to  scalar  measurement  processing  as 
discussed  in  Section  6.1.  If  the  measurement  covariance  matrix  is  nondiagonal, 
a linear  transformation  must  be  performed  on  the  measurement  vector  in  order 
to  decorrelate  the  measurement  noise  components. 

The  extrapolation  operation  can  partially  or  totally  destroy  the  triangularity 
of  S,  and  as  a consequence  a retriangularization  procedure  may  be  required. 

To  illustrate  the  use  of  Carlson  algorithm  consider  the  m-dimensional  measure- 
ment vector  variation  at  time  t^, 

6yi  = Hi6xi  + vi 

A 

with  measurement  error  covariance  Rj.  It  Is  assumed  that  6xT  and  ST  are 
available  where  Sj  Is  a triangular  square  root  of  P^. 

If  is  nondiagonal,  the  first  step  Is  decorrelate  the  measurement  noise 
components.  This  is  accomplished  by  applying  Gaussian  elimination  with 
complete  pivoting  (Cf.  Appendix  D)  to  R^.  This  procedure  yields  a lower 
triangular  matrix  L and  an  upper  triangular  matrix  U such  that 

LJlR^n1  = U , 

where  n Is  an  invertible  matrix  constructed  by  row  permutations  on  the 
identity  matrix. 


[' 

I 

t • 
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The  matrix  L has  all  ones  on  Its  main  diagonal,  and  thus,  by  virtue  of 
triangularity,  L"1  exists. 

Non  consider  the  transformed  measurement  equation 
ay1  " Vxl  + vl 


where 


6yi  * Ln«yi 
Hi  * LnHj  , 
vj  * Lnvi  , 

l 

and  the  covariance  of  v^  Is  simply 


Ln^nV  • ULt  . 


Using  the  fact  that  (Ln)"1  exists,  it  is  easily  shown  that  the  standard 

I I I 

filter  update  with  6y^,  H.. , R..  is  entirely  equivalent  to  update  with  6y.. , 

, R.j.  But  it  is  readily  seen  that  R^  is  diagonal,  since  it  is  both 
symmetric  and  upper  triangular.  (R..  is  symmetric  since  it  is  a covariance 
matrix,  and  it  is  upper  triangular  since  it  is  the  product  of  two  upper 
triangular  matrices,  U and  LT.)  In  fact,  element  by  element,  the  main 
diagonal  of  R^  is  equal  to  the  main  diagonal  of  U.  That  is. 


As  a consequence  of  R^  being  diagonal,  the  transformed  measurements  can  be 
processed  by  the  scalar  measurement  update  procedure  of  Section  6.1. 


i 


i 


i 
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The  next  step  is  to  process  the  individual  scalar  components  of  the  trans- 
formed measurement  vector  using  the  Carlson  update  algorithm.  Suppressing 
both  the  subscript  i and  the  prime  notation,  let  the  transformed  measurement 
quantities  be  given  by 


6y  = 

6yl 

6y2 

, H = 

V 

h2. 

, R - 

\0 

1 

«< 

3 

L 

H. 

m 

O 

m « 

The  scalar  update  procedure  consists  of  applying  the  Carlson  update  j 

equations  sequentially  to  the  elements  of  6y.  The  combined  decorrelation 
and  Carlson  update  procedures  are  illustrated  in  Figure  6.3. 

In  the  figure  dummy  variables  have  been  used  for  intermediate  results  which 

need  not  be  saved.  The  starred  box  contains  the  key  calculations  of  the 

Carlson  update  procedure,  and  the  detailed  algorithm  for  the  functions  in 

this  box  is  provided  in  Appendix  A.  Notice  that  the  matrix  Ji  of  measurement 

partial  derivatives  with  respect  to  constrained  states,  required  for  error  j 

propagation  (Cf.  (43),  (56)  of  Section  5.0),  is  also  subject  to  the  decorrelation 

transformation  L.  The  quantities  which  must  be  saved  for  smoothing  and  error 

propagation  functions  include  H,  J,  Kp  , j = 1,  ...,  m,  and  R in  addition 

to  6xT,  6x.j , ST,  and  S. . 

The  final  step  is  the  extrapolation  operation.  The  nominal  state  and  the 
state  variation  are  extrapolated  in  the  same  way  in  the  square  root  filter 
as  in  the  standard  filter.  To  extrapolate  S-,  a triangular  matrix  ST+^ 
must  be  found  such  that 


Pi+1  = Si+l^Si+l)T 


where  (Cf.  (28),  Section  5.0) 


p;+!  * wi  * «i  ■ 
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and 


pi  - sisi  • 

This  could,  of  course,  be  accompl i shed  by  computing  P^  directly  and  applying 
the  Cholesky  decomposition  to  obtain  Sj+,j.  But  this  would  defeat  the  purpose 
of  the  square  root  filter  which  Is  to  avoid  sequential  computation  of  P and 
thereby  gain  certain  advantages  with  respect  to  computational  efficiency, 
numerical  precision,  and  filter  stability. 

The  indirect  calculation  of  ST+^  is  performed  in  the  following  manner.  Let 
ri  be  a square  root  of  Q^,  and  observe  that 

[Vi  i tVi  ! VT  * Vi ‘I + Qi  - pHi  • 

Thus  the  augmented  matrix  [$.S.  ! r.]  satisfies  one  property  required  of 

_ I 1 j 1 

but  it  is  not  square. 

-1  T 

Now  let  T be  orthogonal  matrix,  i.e.,  T = T , of  the  proper  dimension  to 
allow  pre-multiplication  by  I\|]  and  observe  that 

([*,3,  : r,]T>  fC^s,  ; r.]T}^  = C^s,  : r,]  [^s,  : r,]7  = p:+1  . 

Thus  if  T is  selected  such  that 


[*isi  • T\iJ  m cs‘  ' o]  , 

then  S'  is  a square  root  of  PT+^. 

By  applying  the  modified  Gram-Schmidt  (MGS)  process  (Cf.  Appendix  D)  to  the 
rows  of  [$jSi  ! r.],  augmented  by  the  rows  of^  jj,  an  orthogonal  matrix  T 
can  always  be  constructed  such  that 


C«1si  I ri]T  = [S'  ! 0]  , 


where  S'  Is  triangular.  The  order  in  which  the  rows  of  [4..S^  ! r^J  are 
processed  to  obtain  T determines  whether  S'  is  upper  triangular  or  lower 
triangular. 

In  the  particular  case  where  = 0,  it  is  clear  that  the  matrix  4>.S..  is 
itself  a square  root  of  PT+^.  However,  depending  on  the  structure  of  4^. , the 
product  is  not  necessarily  triangular.  Consequently,  even  when  = 0, 
the  MGS  process,  applied  to  the  rows  of  4>1-S1.  augmented  by  the  rows  of  I if 
necessary,  may  still  be  required  to  obtain  a triangular  square  root  of  P7+^. 

In  this  case  the  procedure  is  appropriately  referred  to  as  retriangularization. 

The  efficiency  of  the  square  root  covariance  extrapolation  is  greatly  influenced 
by  the  structure  of  4^.  and  which  in  turn  is  dependent  on  the  ordering  of  the 
elements  within  the  estimated  state  vector. 

To  illustrate  efficient  extrapolation,  assume  that  the  upper  triangular  form 
of  S has  been  selected  and  that  x,  4,  and  Q can  be  partitioned  as  follows: 


This  form,  which  can  always  be  achieved  by  proper  ordering  of  the  elements  of 
x,  has  grouped  all  dynamic  states  which  are  driven  by  noise  in  xa,  all  dynamic 

a 

states  which  are  not  driven  by  noise  in  xb,  and  all  static  states  in  xc- 
Denote  the  respective  dimensions  of  xa>  xb,  xc  by  ng,  nb,  nc. 

The  corresponding  partitioned  form  of  S is  given  by 


S = 


S S L S 
aa  ab  ac 


0 

0 


Sbb  Sbc 


cc 


where  Saa,  Shh,  and  Srr  are  each  upper  triangular. 
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'bb 


cc 


Now  define 


z * $s  . 


Then,  In  partitioned  form 


z 


Eaa  Eab  Eac 
0 Ebb  Ebc 


cc 


where  Zaa  and  Zbb  are  not  necessarily  triangular.  However,  Zcc  is  upper 
triangular;  in  fact  Z = S . 

LL  tv 

Using  the  above  partitioned  forms  it  follows  that 


Eaa 

Zab 

Eac 

raa 

0 

1 

o 

[*s  : r]  = 

0 

Ebb 

Ebc 

0 

0 

0 

0 

0 

Ecc 

0 

0 

0 

where  is  a square  root  of  Q__. 

da  ad 

Now  an  orthogonal  matrix  T,  such  that 

[z  : r]  t = [z-  : o]  , 


where  z"  is  upper  triangular,  can  be  computed  in  partitioned  form  as  follows: 
1 . Apply  the  MGS  process  to  the  rows  of 


I 

1 

1 

0 

0 

1 

1 

• 

I 

z 

1 

r 

aa 

1 

aa 

proceeding  in  the  order  from  bottom  to  top,  skipping  linearly  dependent  rows,  and 
stopping  when  nfl  orthonormal  vectors  have  been  obtained.  Denote  by  [t|,  ! T^] 


the  matrix  whose  rows  are  given  by  the  orthonormal  vectors  placed,  from 

w i 

bottom  to  top,  in  the  order  in  which  they  are  computed. 

T 1 T T 1 T 

The  matrix  [T^  ! T^j]  is  nax2nfl.  Denote  by  [T14  j T^]  the  nax2nfl  matrix 

whose  rows  are  given,  in  any  order,  by  the  na  orthonormal  vectors  which  were 

a 

not  computed. 

2.  Apply  the  MGS  process  to  the  rows  of 


V 


proceeding  in  the  order  from  bottom  to  top,  skipping  linearly  dependent  rows, 
until  the  full  set  of  nb  orthonormal  vectors  is  obtained.  (In  the  special 
case  where  ng  = 0,  the  process  can  be  terminated  when  the  rows  of  Ebb  are 
exhausted.)  Denote  by  the  matrix  whose  rows  are  given  by  the  nb  orthonormal 
vectors  placed,  from  bottom  to  top,  in  the  order  in  which  they  are  computed. 


The  complete  matrix  T is  given  by 


\ 

( 


but  only  T^,  Tg2»  T^  are  actually  computed.  Now  the  resulting  product 

1 

| [z“  : o]  = [z  : r]  t , 

t 

i 


* 
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with  upper  triangular  Z , is  expressed  in  partitioned  form  by 


’£ia 

Eab 

e;c  " 

M 

1 

II 

0 

Ebb 

Eic 

1 

o 

0 

zcc  . 

where 

Eaa  = EaaTll  + raaT41 
Eab  s EabT22 
Ebb  = EbbT22 


cc 


cc 


The  complete  square  root  covariance  extrapolation  is  illustrated  in  Figure  6.4. 

The  Inputs  are  #. , S..,  and  Q. , and  the  output  is  S7+1>  The  partitioned  form 

of  computation  is  very  efficient  in  those  cases  where  n + n.  « n . Note  in 

d D C 

particular  that  the  partitioned  block  of  S represented  by  Scc  does  not  change 
during  the  extrapolation  process. 

6.3  Computation  of  Nominal  State  Vector  and  Transition  Matrix 

The  mathematical  development  of  Section  5.0  requires  that  solutions  be  obtained 
to  several  differential  equations.  The  state  vector  differential  equation  is 
integrated  to  obtain  the  nominal  state  vector.  Then  the  state  vector  differential 
equation  is  linearized  about  the  nominal  state  and  this  linear  equation  is  solved 
to  obtain  the  transition  matrix. 


i 


Let  the  state  vector  be  permuted  and  partitioned  into  dynamic  states  x , static 

d 

states  xh  which  affect  the  dynamic  states,  and  static  states  x.  which  do  not 


r 


affect  the  dynamic  states.  (Note  this  is  not.  the  same  partition  as  used  in 
Section  6.2.)  Then  the  differential  equation  for  the  nominal  state  vector 
is  given  by 


(1) 


1 

X- 

o> 

1 

faCxa(t).  V t] 

*b 

II 

0 

1 

U 

•X 

1 

i 

o 

1 

, t1  < t < t'  , 


where  the  notation  (~)  has  been  suppressed. 


Linearization  of  (1)  about  the  nominal  state  vector  leads  to  the  differential 
equation  for  the  fundamental  matrix 


(2) 


'l'aa(t>  ',ab<t>  ',ac<t> 
Tba(t>  V1'  'ibc<t> 


*ca<t>  *cb(‘)  »cc(t) 


F.a<‘>  Fab">  0 
0 0 0 

0 0 0 


’,aa<t>  '1'ab<t>  V1* 
,,ba‘t>  V1*  V1* 


'*'ca<t>  '*'cb<t)  f-lt> 


cc 


with  initial  condition 


■'|,aa<t'> 

W1') 

V1')' 

"i 

0 

0 ~ 

V(t') 

= 

0 

I 

0 

/ca'1') 

W'l 

0 

0 

I 

where 


aa 


3t" 

3x 


a_ 

T ’ 
a 


ab 


3f 

l 

3x 


, t1  < t < t"  , 
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It  is  clear  that  *ac(t)  = 0,  ¥ha(t)  = 0,  fhk(t)  = I,  fKr(t)  = 0,  y„a(t)  = 0, 


ba 


bb' 


rbc' 


ca' 


fcb(t)  = °»  ',,cc(t)  = 1 for  a11  * 1n  Ct'»  t,,]* 

Consequently, 

(3)  *,.<»>  * Faa(t),,.,(t>-  f 1 1 < t"  . 
and 

(4)  fab(t)  = Faa(t)yak(t)  + Fak(t),  t*  < t < t"  . 


aav  7 ab' 


ab' 


Once  the  solution  of  (3)  has  been  obtained,  (4)  can  be  solved  by  direct 
integration.  Thus 

t 

<5>  ,,ab(t>  * / ,,aa(t),,M(r)Fab(r)dr  • 

f 

Now  the  transition  matrix  is  given  by 


*aa(t’s)  »ab(t-s)  *ac(t's> 

',aa(t)  ',ab(t)  0 

’aa(s)  ',ab<s>  0 

(6) 

‘ba(t-s)  Wt>s)  V!t,s) 

= 

0 I 0 

0 I 0 

_«ca(t's)  *cb(t’s)  *cc(t's) 

0 0 I 

— — 

0 0 I 

-1 


for  all  t,  s in  [t',t"].  From  (6)  it  follows  immediately  that 


(7) 


$(t,s)  « 


*aa<t>s>  »ab(t>s)  0 


0 

0 


I 

0 


0 

I 


where 


..-1, 


(8)  *M(t.s)  = »aa(t,,,Ia(s) 
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and 


m 


(9)  tab(t,s)  = »ab(t)  - *„(t.»)»ab(s) 

for  all  t,  s In  [t‘,  t"]. 

To  summarize  the  procedure  for  constructing  the  nominal  state  vector  and  the 
transition  matrix,  denote  the  dimensions  of  xa,  xb,  xc  by  na,  nfa,  nc, 
respectively.  The  steps  are: 

1.  Given  the  nominal  state  initial  condition 


\(ff 

1 

X 

0*  o 

1 

Xb(f) 

II 

< 

1 

+J 

X 

1 

A. 

solve  the  nfl  - vector  differential  equation 

*a(t)  = fa[xa{t)»  xb»  tL  xa(t,)  = XS*  f < t < t" 
and  put  xb(t)  = x£,  x£(t)  = x°  for  all  t in  [t* , t"]. 

2.  Evaluate  the  partial  derivative  matrices 


3f. 


on  [t* . t"]  using  the  nominal  state  vector. 


bataWfotfS  - i?*<i*At*' ' 


3.  Solve  the  n _xn_  - matrix  differential  equation 

a a 

V1'  = F.«<t>',M<t>*  ',^t')  ’ I.  t'<t<t"  . 

4.  Compute  *F~^(t)  on  (t‘,  t")  by  direct  matrix  inversion  or,  alternatively, 
by  solving  the  differential  equation 

*«<*>  = -‘i'aa(t)Faa(t1’  ‘*  < t < t"  . 

5.  Compute  4>_a(t,s)  for  t,  s in  [f,  t"]  by 

aa 


4aa(t-5>  * Taa(t,'Ws> 


6.  Integrate  the  naxnb  matrix 


L 

t^Xb^*  - / '!,al(r)Fab<r)dr>  v £ 1 < *"  • 


7.  Compute  $ab(t,s)  for  t,  s in  [t1,  t"J  by 

*»h(t.s)  = T„(t)  {[^^(t)  - C^I^JCs)}  . 


aa  abJ 


8-  Put  *ac(t,s)  = 0,  $ba(t,s)  = 0,  *bb(t,s)  = I,  4>bc(t,s)  = 0,  $ca{t,s)  = 0, 
<J>cb(t>s)  = 0,  $cc(t,s)  = I for  t,  s in  [t‘,  t"]. 

In  the  above  procedure,  standard  numerical  integration  methods  can  be  used 
to  obtain  solutions  to  the  differential  equations  at  discrete  points  in  the 
interval  [t‘,  t"].  Then,  standard  interpolation  methods  can  be  used  to 
obtain  solutions  at  arbitrary  points  in  [ t ' , t"]. 

The  results  of  a TRAM  related  computer  study,  in  which  a fourth  order 
Runge-Kutta  integration  method  was  used  and  in  which  fifth  order  spline 
functions  were  used  for  trajectory  interpolation  and  third  order  spline 
functions  were  used  for  interpolation  of  the  fundamental  matrix,  is  reported 
in  [2].  The  study,  which  was  limited  to  an  investigation  of  a freefall 
case,  demonstrated  that  (with  a step  size  of  5 seconds)  the  integration 
algorithms  and  interpolation  methods  were  efficient  and  sufficiently 
accurate  for  TRAM  applications  in  freefall. 
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6.4 


Numerical  Calculation  of  Partial  Derivatives 


Partial  derivatives  expressions  are  required  in  conjunction  with  the 
linearization  of  the  state  and  measurement  equations.  If  a function  to  be 
differentiated  is  represented  by  a closed  form  analytical  expression,  its 
derivatives  can  be  calculated  directly  using  standard  formulas  and  the  chain 
rule.  If,  on  the  other  hand,  the  function  in  question  cannot  be  expressed 
in  closed  form,  then  numerical  calculation  of  some  of  its  derivatives  may 
be  required. 

In  some  cases  where  direct  calculation  of  derivatives  by  formula  is 
feasible,  it  may  be  undesirable  for  several  reasons.  First,  in  order 
to  use  direct  methods,  analytical  expressions  for  each  partial  derivative 
must  be  developed,  and  then  these  expressions  must  be  programmed.  Often, 
this  is  a tedious  and  error  prone  process  for  analyst  and  programmer  alike. 
Second,  in  many  cases,  the  complexity  of  partial  derivative  expressions 
greatly  exceeds  that  of  the  primative  function,  and  consequently  the  execution 
time  required  for  derivative  evaluations  can  greatly  exceed  that  required  for 
prime  function  evaluation. 

Calculation  of  partial  derivatives  by  numerical  methods  can  be  accomplished 
very  simply  using  only  primative  function  evaluations.  These  functions  must 
be  programmed  regardless  of  how  their  derivatives  are  calculated,  and  the 
additional  analysis  and  programming  required  for  numerical  differentiation 
is  trivial.  However,  caution  must  be  exercised  to  prevent  the  introduction 
of  excessive  amounts  of  either  roundoff  or  truncation  error. 


To  illustrate  the  numerical  differentiation  procedure  and  bound  its  errors, 
let  g be  a function  whose  derivative  at  zQ  is  approximated  by 


9'(*0) 


{ 


g(z0  + a)  - g(zQ) 


for  some  a > 0. 


!■ 
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The  roundoff  error  in  this  approximation  is  given  by 


r[g'(z0)]  = £ {r[g(zQ  + a)]  - r[g(z0)]> 


where  r(g)  denotes  the  roundoff  error  in  the  evaluation  of  g. 


If  g'  is  continuous  on  [zQ,  zQ  + a]  and  g"  is  finite  on  (zQ,  zQ  + a),  the 
truncation  error  in  the  approximation  of  g ' (zQ)  is,  by  Taylor's  formula 
with  remainder, 

t[g'(z0)]  = f g"(z1) 

for  some  z1  in  (zQ,  zQ,  + a). 

Observe  that  roundoff  error  magnitude  increases  with  decreasing  a,  while 
truncation  error  magnitude  increases  with  increasing  a.  Consequently, 
judicious  selection  of  a is  essential  in  order  that  the  total  enror  in 
the  approximation  to  g'(z0)  not  be  excessive. 

When  g'  is  required  in  conjunction  with  a linearization  procedure,  a 
rationale  for  the  selection  of  a can  be  developed. 


Linearization  of  a functional  relation 
w = g(z) 

about  a point  z implies  that  the  approximation 
w 3 g(z)  + g'(z)6z 

is  invoked,  where  6z  = z - z.  When  this  representation  is  used  for 
computation  there  are,  in  addition  to  the  error  in  g'(z),  errors  in  w 
due  to  both  roundoff  and  truncation.  Excluding  any  error  in  g'(z),  the 
roundoff  error  in  w is  given  by 

r[w]  = r[g{z)]  , 
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and,  assuming  g'  is  continuous  and  g"  is  finite,  the  truncation  error  in 
w is  given  by 


C: 

i- 

> 


t[w]  = j g"(c){6z)2 

for  some  c interior  to  the  interval  joining  z and  z. 

Now  let  zQ  = z and  assume  g"  is  continuous  on  some  closed  interval  [a,  b] 
containing  zQ,  zQ  + a,  and  z.  Define 

I | r[g] | | = max  |r[g(c)]| 
a < ? < b 

and 

1 1 9"  II  * "»ax  |g"U)  | . 
a < ? < b 

Then  the  total  roundoff  and  truncation  error  in  the  representation  of  w is 
bounded  by 

|e(w)|  < ||r[g]||  t J 1 1 g" 1 1 (SZ)2 

♦ {|  lk[g]||  + f l|g"ll}  (6Z)  , 

where  only  the  terms  involving  a are  due  to  error  in  g 1 ( zQ ) . 

Now  suppose  a = 6z.  Then 

|e(w) | < 3||r[g]||  + ||g"||  (5z)2  . 

Thus,  with  a * 6z,  the  roundoff  error  bound  does  no  worse  than  triple,  and 
the  truncation  error  bound  no  worse  than  double,  when  errors  in  g ' are  added 
to  the  errors  which  already  exist  in  w.  Consequently,  if  the  algorithm  for 
evaluation  of  g has  negligible  roundoff  error,  and  if  the  linearization  of 
g has  negligible  truncation  error,  then  g1  has  negligible  roundoff  and 
truncation  error  when  a ■ <5z. 


I 


4 


The  above  discussion  shows  that  when  g' (zQ)  is  approximated  by 

, , 9(z0  + <*)  - g(z  ) 

9'UJ  * — 2 r- . 


the  roundoff  error  is  bounded  by 


IKg']|  1 | ||r[g]||  , 


and  the  truncation  error  is  bounded  by 


|t[g‘]|  < | I |g"| 


if  g'(z0)  is  approximated  by  the  symmetric  formula 


g'(z0) 


g(zQ  + a/2)  - g(zQ  - a/2) 


it  can  be  shown  that  the  roundoff  error  is  likewise  bounded  by 
I r[g ■ J | < | ||r[g]||  , 

and  the  truncation  error  is  bounded  by 

i I |9'"II 

provided  gm  is  continuous  on  [a,  b]  (this  time  selected  to  contain 
zQ  + a/2  and  zQ  - a/2). 

The  roundoff  error  bound  is  the  same  for  the  two  formulas  for  g-(z  ),  but 
the  truncation  error  is  one  order  higher  in  the  symmetric  formula.  Consequently 
in  comparison  with  the  truncation  error  of  the  linearization  process,  the 
truncation  error  of  the  symmetric  differentiation  formula  should  be  negligible. 

Since  g(z)  is  required  in  the  linearization  procedure,  calculation  of  g'(z) 
by  the  unsymmetric  formula  requires  only  one  additional  evaluation  of  g. 
Calculation  of  g'(z)  by  the  symmetric  formula  requires,  two  additional 
evaluations  of  g,  but  the  additional  computation  is  worth  consideration 
because  of  the  truncation  error  protection  it  affords. 


The  use  of  a = 62  in  the  above  discussion  was  not  intended  for  any  purpose 
other  than  error  analysis.  In  practice  it  is  desirable  to  select  fixed 
values  of  a for  each  variable  with  respect  to  which  numerical  partial 
differentiation  is  to  be  performed.  For  each  state  variable,  a value  of 
a roughly  equal  to  the  magnitude  of  the  anticipated  estimation  error  in 
that  variable  should  be  selected. 

6.5  Transit  Time,  Refraction,  and  Doppler  Calculations 

Let  t'  denote  the  time  at  which  a signal  leaves  a target  vehicle  and  let  t 
denote  the  arrival  time  of  the  signal  at  a sensor  which  is  tracking  the 
vehicle.  The  time  difference 

t = t - t' 

is  called  the  transit  time  from  target  to  tracker. 


The  apparent  range  to  the  target  at  time  t'  is  defined  in  terms  of  transit 
time  by 

RA(t')  = ct  , 

where  c is  the  speed  of  light  in  a vacuum.  The  apparent  range  RA(t') 
and  the  true  range  R(t')  differ  only  because  of  refraction. 

If  the  true  range  and  the  range  refraction  error  are  known  functions,  then 
both  transit  time  t and  time  of  transmission  t'  can  be  computed  for  any  time 
of  reception  t by  applying  Newton's  method  to  the  function 

g(t')  = RA(f)  - c(t  - f)  . 

This  results  in  the  algorithm 


Ti+1  = Ti  + 


Rfl(t  - T.)  - CT,. 


C + RA(t  - T.j ) 


which  is  solved  iteratively  beginning  with  tq  = 0 and  ending  when  the 
condition 


t 

?■ 


lTi+l  - Til 

is  satisfied  for  an  arbitrary  n > 0,  or  alternatively,  when  two  successive 
values  of  t differ  by  less  than  computer  roundoff  error. 

In  a two  way  doppler  system  in  which  the  vehicle  transponds  (repeats  an 
exact  replica  of  the  signal  received)  with  negligible  delay,  the  doppler 
frequency  is  defined  as  the  difference  between  the  transmitted  and  received 
frequencies  at  the  tracker.  This  difference  frequency  is  expressed  by 

°<‘>  • h at  <"<*>} 


_ 1 d_ 
2xr  dt 

= ]_  d_i 

2u  dt 

j 2wf0(t  - 2t)  - 

' -»o3l 

where  fQ  is  the  transmitted  frequency. 
Now,  since  x satisfies  the  relation 
ex  = RA(t  - x)  , 
it  follows  that 

C % - RA(t  - X)  [1  - . 

Thus 

^ , Rft(t  - t) 
dt  c + Rfl(t  - t) 

and  consequently 


* 


1 


? 

i 


i 


i 


} i 

* 

■ i 
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D(t) 


C + R A ( t 1 ) 


t'  = t - T 


The  transit  time  and  doppler  calculations  Involve  apparent  range  and  apparent 
range  rate,  which  in  turn  involve  refraction  errors. 

Under  the  usual  assumptions  regarding  time  invariance  and  spherical  symmetry 
of  the  refracting  medium  it  follows  that 

RA  = R + p[R,  E] 

Aft  - A 

Eft  = E + e[R,  E] 

D = D + ^2.  D + Ifi.  C 

RA  K + 3R  R + 3E  E 

where  R,  A,  E,  are  true  range,  azimuth,  and  elevation  of  the  target  relative 
to  the  tracker,  p and  e are  range  and  elevation  refraction  errors,  and  RA> 

Aa,  Ea  are  apparent  range,  azimuth,  and  elevation.  Observe  that  p and  e 
are  functions  of  true  range  and  elevation  only. 

The  usual  method  of  computing  refraction  errors  involves  "ray- tracing"  through 
the  refractive  medium  with  an  assumed  spherically  symmetric  refraction  profile. 
Viewed  as  a black  box,  such  a refraction  algorithm  performs  in  the  following 
manner.  The  inputs  are  the  apparent  values  RA  and  EA,  and  the  outputs  are 
the  true  values  R and  E together  with  the  errors  p and  e.  Within  the  black 
box  operation  begins  by  tracing  a ray  from  the  tracker  with  elevation  angle 
Ea  and  continuing  until  the  apparent  range  along  the  ray  equals  RA.  Then  R 
and  E are  computed  from  the  endpoints  of  the  ray. 

Now  if  the  refraction  algorithm  produces  smooth  values  of  p and  e,  the 
algorithm  can  also  be  used  to  obtain  numerical  partial  derivatives  of  p and  e 
with  respect  to  R and  E.  However,  since  the  inputs  to  the  refraction  algorithm 
are  RA  and  EA,  rather  than  R and  E,  it  is  necessary  to  employ  the  chain  rule.  Thus 
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S>£- 

" 3RA 

3Ra  " 

3R 

3E 

3RA 

9EA 

3R 

3E 

3e 

3c 

3e 

3e 

3EA 

3EA 

3R 

3E 

_> 

_ 3^ 

3EA  _ 

3R 

3E 
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3£ 
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3£_ 

3R 

3R 

3R 

3£ 

3Ra 

3EA 

3Ra 

3EA 

3e 

3R 

3e 

3E 

3e 

_ 3RA 

3e 

3EA 

3E 

_ 3RA 

qpIq^ 

mm 

1 

l 

But  since 


3p  <1  8_R  3p  3R 

3Ra  3Ra  • 3Ea  ' ' 3Ea  ’ 

3e  3E  3e  „ , 3E 

3^  '3^*3^  * 


it  follows  that  only  p,  e,  and  the  partial  derivative  matrix 


3R 

3R 

3RA 

3E 

3E 

3Ra 

3EA 

need  be  computed. 

The  complete  procedure,  using  the  symmetric  central  difference  formula, 
is  outlined  below. 


1.  At  the  tracker,  trace  a ray  with  elevation  angle  EA  to  apparent  ranges 
Ra  - a/2,  Ra  and  RA  + a/2,  and  compute  the  corresponding  true  ranges  and 
elevations: 
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R‘,  R,  R , 


E",  E,  Et  . 


2.  Put  p = R.  - R,  e = E.  - E 


3R  R+  - R"  3E  E+  - E‘ 


3RA  a * 3RA  a 

3.  At  the  tracker  trace  a ray  with  elevation  angle  EA  - a/2  to  apparent 
range  RA  and  compute  the  corresponding  true  range  and  elevation:  R",  E“. 

4.  Repeat  3.  with  elevation  angle  E.  + a/2  and  compute  the  true  range  and 

+ + ™ 
elevation:  R , E . 


5.  Using  the  results  of  3.  and  4.  compute 


3R  R+  - R“  3E  _ E+  - t 


c Du-#-  a - /3R  \ / 9E  \ /3E  \ 1 3R  > 

6.  Put  A - (aR^)  (3E^)  - (3R^)  (3E^) 


= (1)  (li_)  . i 

3R  V l3E.;  1 


= _ (1)  (°LR-  ) 
3E  V 'SE^ 


= _ (1) 

3R  V l3RA; 


M = /I)  /IfL)  _ I 
3E  V l3Rft'  1 ’ 


6.6  Measurement  Processing 

The  measurement  variational  equation  for  a four  channel  metric  tracking 
radar  is  given  by  (19)  in  Appendix  C.2,  and  is  repeated  here  for  convenience. 


fiy(t) 


- / 3Ra  \ 

h.  - — M-  ( — f ) fix  (t1 } 

c + R*  \dXa/ 

A 


+ Hk6x.  + H.6x^ 
b b c c 


Cw  •*  T ** 
U tlx'ft 


- Ha*ams(t)6xd 


c + RA 


+ ~V  v;m^,)6xe 

c + ra 


KK  /3Ra\ 

+ H-  - ■■■—■  ( -t  1 fix- 

* * \ 1 / T 
c + R^  \3Xf/ 


Hi'  /8R.  \ 

, §_§_  ( 6x„ 

9 * \ ax^  / 9 

C + \ dXg/ 


+ v(t)  . 

The  variation  6y  is  formed' as  theivector  difference  of  the  actual  and 
nominal  measurements. 


fiy  s y-y  = 


But  if  only  the  encoder  measurements  are  available  for  processing,  the 
dynamic  error  terms  replace  the  actual  measurements,  and 


—*■  ate  -hi  i tw# 


6y  = y-y 


The  expressions  for  the  dynamic  error  terms  are  given  in  Appendix  C.3. 

The  nominal  measurement  vector  y is  computed  by  evaluating  the  measurement 
equation  using  nominal  values  for  all  states.  The  form  of  the  measurement 
equation  which  is  suitable  for  this  purpose  is  called  the  dynamic_ measure- 
ment equation.  The  algorithm  for  the  dynamic  measurement  equation  is 
given  below.  The  inputs  are  the  encoder  values  Rp  Ap  Ep  Rp  sensor 
indicated  measurement  time  0S,  and  nominal  values  of  state  variables.  It 
is  assumed  also  that  the  range  and  elevation  refraction  corrections  and 
their  respective  partial  derivatives,  together  with  the  true  range  and 
elevation  at  which  they  apply,  have  been  obtained  by  the  procedure  outlined 
in  Section  6.5.  These  quantities  are  denoted  by  p,  e,  |^-,  |jr,  R,  E, 

respectively.  The  outputs  of  the  algorithm  are  the  nominal  measurement 
variables,  r,  <j>,  i|»,  d. 

6.6.1  Dynamic  Measurement  Algorithm 

1.  Correct  sensor  time  by  solving 


0S  = t + m^  (t)  xd 


for  t,  iteratively  if  necessary  by  Newton's  method. 

2.  Compute  the  coordinate  transformation  from  the  geocentric 
to  the  topocentric  system  at  the  sensor  site  using  astro- 
nomic longitude  A A and  latitude  ©A: 


CT  = 
G 


-sin  A. 


-sin  ©A  cos  AA 


cos  0A  cos  A A 


cos  A. 


-sin  0A  sin  aa  cos  eA 


cos  0A  sin  A A sin  ©A 


3. 


Compute  the  geocentric  coordinates  of  the  sensor  site  using 
the  geodetic  longitude  Xg,  latitude  0Q,  and  height  Hg: 


a cos  0, 


Xs  = 


^1-e2  sin2  0, 


a cos  0, 


cos  Xr  + HP  cos  0r  cos  Xr, 


sin  Xr  + Hr  cos  0r  sin  Xr, 


1-e  sin  0, 


a (1-0  sin  0, 


sin*  0, 


+ Hg  sin  0g, 


where  a is  the  mean  equatorial  radius  of  the  ellipsoid  and 
e Is  its  eccentricity. 


4.  Apply  the  iterative  procedure  developed  in  Section  6.5  to 
solve  for  transit  time,  refraction  and  doppler: 


(i)  Initialize:  x = 0,  t'  = t. 

( ii ) In  the  nominal  trajectory  time  tags  have  been  corrected 
for  nominal  vehicle  timing  errors,  put  t"  = t' ; other- 
wise t"  * t'  + mj  (t‘)  xg  . 

{iii)  Interpolate  the  nominal  trajectory  to  t"  obtaining 


geocentric  coordinates  x,  y,  z,  *,  y,  2,  and  compute: 


ffr'ifojfrte—fov*  ■ ■ s -jssizi 


**  1 

A = tan^  (u,  v)  , 
E = sin-1  (w/R)  , 


R = (uu  + vv  + ww)/R  , 


~2  ~2 
u + v 


• - Mi 


( iv ) Compute  apparent  coordinates  in  presence  of  refraction 
p = p + If  (R  - R)  + H-  (E  - E) 


e = e + !§-  (R  - R)  + ||-  (E  - E) 


le.  r + l£-  f 

n *>  - r *- 


Rft  = R + p 


Aa  - A 


EA  = E + e 


R^  = R + p . 


(y)  Compute  transit  time  increment: 


St  = 


Ra  - CT 


c + R. 


(vi)  Test  for  convergence: 


IF  (fix  = 0)  GO  TO  (viii). 
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(vii)  Adjust  transit  time  and  retarded  measurement  time  and 
repeat  above  steps: 

T T + 6X 
t'  = t - X 

GO  TO  [a). 

[viii)  Compute  apparent  doppler  shift: 


-2f  R. 
O A 


c + R. 


where  f is  the  transmitter  frequency. 


5.  Apply  target  dependent  errors  at  t‘: 


Ra  + ARj 


'a  * “t 


Ea  ♦ AEt 


°A  + 1DT 


6.  Recover  doppler  encoder  value: 

-2fA 

n 0 E 


c + R, 


7.  Correct  encoder  angles  for  encoder  errors. 


A$  = Aj.  - AA^.  , 


E$  = EE  - AE£  , 


iteratively  if  necessary  using  Newton's  method. 


1 


8.  Denote  the  tracker  outputs  by 


oE  , 


apply  the  feedback  errors. 


Rc  = R - ARC 
r Or 


Df  = Dq  - ADf  , 


and  compute  the  topocentric  (locally  level)  to  electronic 

-CD 

boresite  coordinate  transformation,  CLL> 


9.  Compute  the  discriminator  outputs: 


Rr  - RF  , 

°r  - Dp  , 

tan_1(ex/ey)  , 
tan_1(ez/ey)  , 


where 


sinAR  cosER 

-CD  ~ ~ 

Cll  cosAr  cosEr 
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10.  Apply  the  sensor  errors  to  obtain  the  nominal  sensor  outputs: 


i 


r 

r 

% 

r + 

Ar_ 

s 

0 

s 

d 

d_ 

s 

d„  + 

Ad 

s 

0 

s 

-0-  i 

i. 

*0 

sfh 

SF,, 

♦o 

2 

s 

+ 

* 

*s 

*0_ 

_SFn 

SF,, 

♦* 

*0 

6.6.2  Measurement  Equation  Partial  Derivatives 

The  coefficients  of  the  state  vector  variations  in  (1)  constitute  the  total 
or  dynamic  partial  derivatives  of  the  measurement  with  respect  to  the 
respective  state  variables.  Thus 


i 


and 


= H - 
3xg 


C + <A 


The  quantities  Ha,  Hb,  Hr,  and  H^,  appearing  in  the  above  expressions, 
are  called  static  partial  derivatives  of  the  measurement  with  respect  to 
the  respective  state  variables. 

The  total  partial  derivatives  of  the  measurement  can  be  calculated  by 
numerical  differentiation  using  the  dynamic  measurement  algorithm  specified 
in  the  preceding  subsection.  Alternatively,  the  total  partial  derivatives 
can  be  constructed  from  the  static  measurement  partial  derivatives,  where 
the  static  partial  derivatives  are  computed  by  numerical  differentiation 
using  the  static  measurement  algorithm  to  be  given  below.  The  latter 
method  of  obtaining  measurement  partial  derivatives  is  useful  in  conjunction 
with  the  measurement  variation  averaging  processing  method  to  be 
considered  in  Section  6.6.5. 

The  static  measurement  algorithm  is  very  similar  to  the  dynamic  algorithm, 
except  that,  in  addition  to  the  other  inputs,  t and  t’  are  specified,  and 
thus  it  is  not  necessary  to  solve  for  transit  time  by  iteration.  Steps 
in  the  static  algorithm  which  are  identical  with  corresponding  steps  in 
the  dynamic  algorithm  will  be  listed  without  elaboration. 

6.6.3  Static  Measurement  Algorithm 

1.  Compute  the  coordinate  transformation  from  the  geocentric 
to  the  topocentric  system. 

2.  Compute  the  geocentric  coordinates  of  the  sensor  site. 

3.  If  the  nominal  trajectory  time  tags  have  been  corrected 
for  nominal  vehicle  timing  errors,  put  t"  = t1;  otherwise 
t"  = t'  + mj(t' )xg. 

4.  Interpolate  the  nominal  trajectory  to  t"  obtaining  geocentric 
coordinates,  and  compute  R,  A,  E,  R,  E. 
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5.  Compute  apparent  coordinates  R^,  A^,  E^,  R^. 

6.  Compute  apparent  doppler  shift  D^. 

7.  Apply  target  dependent  errors  at  t‘. 

8.  Recover  doppler  encoder  value. 

9.  Correct  encoder  angles  for  encoder  error. 

10.  Apply  range  and  doppler  feedback  errors,  and  compute  the 

-CO 

coordinate  transformation  C£j\ 

11.  Compute  the  discriminator  outputs  rQ,  dQ,  <t>0,  4>0 . 

12.  Apply  sensor  errors  to  obtain  sensor  outputs  rs,  ds,  <{>s,  ^s. 


In  using  either  measurement  algorithm  for  the  purpose  of  obtaining  partial 

derivatives  by  numerical  differentiation,  computational  efficiency  can  be 

enhanced  by  judicious  application  of  the  chain  rule.  For  example,  if 

3 = x . denotes  sensor  timing  bias,  it  follows  that 
al 


= 1 3 mI(t)  ’ 

dxd 

3xT  33  V1  • * 

Thus  differentiation  with  respect  to  the  scalar  3 suffices  to  compute  the 
derivatives  with  respect  to  the  vectors  x^  and  xg. 

6.6.4  Measurement  Processing  with  Adjustable  Estimation  Times 


The  measurement  variation  equation,  which  has  been  the  subject  of  this 
section,  can  be  concisely  written  as  follows 
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, - 


6y(t)  = ^ix  (t')  + 6x  (t) 

»«;  *«;  3*l 

♦ :VX<I<*>  * 5-Sxe<‘'>  * ?*T4xf 


+ Sxa  + ’ 

9xg  9 

For  purposes  of  illustration  all  states  except  those  pertaining  to  survey 
and  refraction  have  been  represented  as  time  varying.  Observe  that  there 
are  three  categories  of  states  in  (2);  those  which  apply  at  t',  those  which 
apply  at  t,  and  those  which  are  constant. 

Now  let  xa  denote  all  states  which  relate  to  the  vehicle  or  its  trajectory 

(including  navigation  and  geopotential  error  states  which  do  not  appear 

in  (2)),  let  x.  denote  all  states  which  relate  to  the  metric  sensor  system, 
p 

and  let  xy  denote  geodetic  and  refraction  error  states.  Then  the  measure- 
ment variation  can  be  expressed  by 

(3)  6y(t)  = ^yfix^t')  + ^fix^t)  + &j8x  (t)  + v(t)  . 


Assume  there  is  no  state  noise,  let  t*  be  arbitrary,  and  observe  that 


$ (t',t*) 

ota  ' 


6xa(t) 

6xy(t) 


Substitution  of  (4)  in  (3)  yields 


0 


«xa(t*) 

5x0(t*) 

6xy(t*) 


«y(t)  = 


6xa(t*) 

^68^***^  ' ^rl  *>> 

I 3xT  001  ! 9x1  33  : 3xT 

L a 3 YJ  6x  (t*) 


+ v(t) 
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In  which  the  measurement  variation  at  time  t is  related  linearly  to  the  state 
vector  variation  at  an  arbitrary  time  t*. 


r 


Equation  (5)  forms  the  basis  for  several  processing  options  in  which  a 
collection  of  measurements  is  used  to  update  the  estimate  of  the  state 
vector  at  an  arbitrarily  specified  time.  For  example  the  state  estimation 
times  can  be  made  periodic,  even  though  the  measurement  times  are  aperiodic. 

It  must  be  remembered,  however,  that  (5)  is  valid  only  in  the  absence  of 
state  noise. 

6.6.5  Processing  with  Measurement  Variation  Average 

A suboptimal  method  of  processing  will  now  be  considered  in  which  the 
measurement  variations,  in  each  channel  of  each  sensor,  are  averaged 
over  a specified  time  interval  and  collectively  used  to  update  the  state 
estimate  at  a specified  time  within  the  interval.  An  important  distinction 
here  is  that  it  is  the  measurement  variations,  not  the  measurement  them- 
selves, which  are  averaged.  \ 

•i 

The  averaging  technique  to  be  developed  here  is  based  on  (5)  of  the 
preceding  subsection  and  consequently  is  valid  only  in  the  absence  of 
state  noise. 

Averaging  of  the  measurement  variations  prior  to  update  of  the  state  vector 
estimate  reduces  the  filter  processing  rate  and  thus  enhances  computational 
efficiency.  Also,  averaging  tends  to  reduce  the  magnitude  of  the  serial 
correlation  coefficients  of  the  filter  input  noise  without  deletion  of 
any  measurements.  Thus  when  serial  correlation  of  measurement  noise  is  a 
potential  problem,  such  as  when  encoder  measurements  are  used  exclusively, 
averaging  can  be  used  to  more  nearly  satisfy  the  requirement  that  the 
measurement  noise  be  serially  uncorrelated  (Cf.  Appendix  A). 

The  reason  that  the  technique  of  measurement  variation  averaging  is  sub- 
optimal  is  that  higher  order  terms  in  a Taylor  series  for  the  averaged 
variation  are  ignored.  The  error  introduced  by  the  neglected  terms  in- 
creases with  the  duration  of  the  time  interval  over  which  the  averaging 
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takes  place.  To  maintain  the  truncation  error  within  acceptable  limits, 
processing  intervals  are  partitioned  into  subintervals  and  the  measure- 
ment variations  on  each  subinterval  are  separately  averaged. 


To  develop  the  technique  of  measurement  variation  averaging,  consider  a 
single  channel  of  an  arbitrary  metric  sensor.  Let  an  interval  be  selected 
over  which  the  variations  are  to  be  averaged.  Let  (t. , iel}  denote  the  set 
of  measurement  times  in  the  Interval,  and  similarly  let  {ti;  iel}  denote 
the  set  of  corresponding  retarded  measurement  times.  Let  tj  denote  the 
state  estimation  time  specified  for  the  interval.  Then  from  (5)  it  follows 
for  each  iel  that 


= Bi«Sx(tj ) + v^)  , 


where 


Bi  - 

: Hr  WV  i 

9X; 

a 

! B ! 

Y 

and 


<Sx(tj) 


5x&(tj)  . 

«xy(tjj 


Since  (6)  Is  written  for  a single  channel,  is  a row  vector.  In  fact, 
B..  is  the  total  partial  derivative  of  y(t^)  with  respect  to  x(tj),  i.e.. 


3y(t1 ) 
3xT(tj) 
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If  Bi  Is  viewed  In  terms  of  Its  constituent  static  partial  derivatives,  as 
evidenced  In  (1),  and  transition  matrix  components,  it  Is  found  that,  for 

<y  * 

fixed  tj,  Is  a function  of  xo(tj),  x^t’),  x0(tf ),  x^).  xy,  tj, 
and  the  encoder  measurement  vector  z(t1). 

• • 

Let  x^,  x^,  Xg,  Xg,  t,  t',  z be  arbitrary  and  expand  B to  first  order  In  (6), 
Thus 


(7) 


6y(tf)  = B6x(tj) 


+ <Sx*  (tj) 


SB 


T ~ ~ 


♦ fr  <x*<‘i>  - x„> 


T ~ 

i • «» 


, 3x 
L_  a 


dB 


T ~ ~ 


* fr(x6(ti>  - x6>  + ZT  <xe(ti>  - V 


ax 


ax 


1§I  — ft'  T'l 

at  'xi  + at1  '*1  " t 7 


+ - z ) 

az1  1 


+ v(t.)  , 


where  B and  its  derivatives  are  evaluated  at  x . '*  , xfl,  xQl  x , t,  t' , z 

»Uh7r,  xY(t,).  P 6 Y 

Now  let  < >j  denote  the  operator  which  performs  simple  averaging  over  I. 
Thus  If  N(I ) is  the  number  of  elements  of  I,  then 


<5‘>i  * HTTT  5f 


Next  put 
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a 


<xa(t!)>I 


<xa(t:)>, 


x6  = <xB(t.)>, 


t 

t‘ 


<t.>, 


I 


<t!> 


I 


<z(t.)>j 


Then  to  first  order 


(8) 


<6y(t. )>j  = B6  x ( t j ) + <v(t. )>j 


Extending  the  above  analysis  to  all  channels  of  a given  metric  sensor  and 
reverting  to  earlier  notation  it  follows  that 


(9) 


<6y (t- )>j  = 


i i 

9xa  j 3xg  ; 3xy 


^Xy  ( 1 1 ) 


+ <V(t. )>J 


where  the  total  partial  derivatives  , ^y  , ^y  are  evaluated  at 


V V V V V 1‘  *'■ z' 


3xa  3XB  3Xy 


Because  of  the  decoupling  of  xa  and  xa,  and  x^,  and  t and  V which 
occurred  in  the  above  development,  it  is  necessary  that  the  total  partial 
derivatives  be  computed  from  static  partial  derivatives 

3Xa  3X6  3Xy 

using  the  expressions  given  at  the  outset  of  Section  6.6.2. 
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i i 
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Finally,  the  measurement  error  covariance  matrix  for  use  in  conjunction  with 
measurement  variation  averaging  must  be  computed.  Extending  (8)  to  include 
the  measurements  from  all  channels  of  a given  sensor  which  are  to  be  processed 
over  the  interval  I,  let 


Rj  j s E[v(ti)v(tJ.)T]  • 


It  then  follows  that 


cov  (<v(t. )>j ) = 


p £ £ R-i  4 

N(IT  lei  jel  3 


In  the  particular  case  in  which  the  measurement  noise  is  serially  uncorrelated, 

cov  (<v(t.)>T)  * — l Rjj  = firry  <R..>r  . 

1 N(I)Z  iel  11  N(I)  1 

6.7  Augmentation  and  Permutation  of  the  State  Vector 

At  any  stage  of  the  filtering  operation  it  is  possible  to  augment  the  state 
vector  with  additional  states.  For  example,  when  an  RV  is  deployed  it  is 
appropriate  to  augment  the  state  vector  with  the  RV  position  and  velocity 
states. 

Moreover,  when  the  filter  operation  is  switched  from  one  RV  to  another,  the 
trajectory  states  of  the  new  RV  become  active  while  those  of  the  old  RV 
become  inactive.  Consequently,  in  order  to  use  the  partitions  defined  either 
in  6.2  or  6.3,  it  is  necessary  to  permute  the  trajectory  states  of  the  two 
RVs  in  question. 


To  Illustrate  the  state  augmentation  procedure  at  RV  deployment,  assume  the 
state  vector  prior  to  augmentation  is  partitioned  in  the  form 


[::] 


where  xa  Includes  all  dynamic  states  of  the  bus  vehicle  (BV)  and  x.  includes 

a C 

all  static  states.  Lex  x^  denote  the  dynamic  states  of  the  RV. 


For  a specified  deployment  configuration,  a separation  model  based  on  energy 
and  momentum  relations  can  be  developed.  In  general  the  model  has  the  form 


where  <&  and  are  known  matrices,  p is  a known  vector,  and  w Is  a zero 

aa  Da 

mean  random  vector  with  covariance 
Q = rrT  . 

6x"  is  the  variation  of  x,  prior  to  deployment,  and  fix*,  6xt  are  variations 

a a a D 

after  deployment. 

The  state  vector  is  augmented  to  include  x^.  Thus 


After  deployment,  the  variation  estimates  are  given  by 


The  square  root  covariance  matrix  prior  to  deployment  is  given  in  upper 

trtenqular  form  by 


The  augmented  square  root  covariance  matrix  after  deployment  can  be  obtained 
by  applying  the  extrapolation  technique  developed  in  Section  6.2  to  the  matrix 


0 

$aaSaa 

^aa^ac 

l 

1 

1 

l 

1 

» 

r 

0 

$baSaa 

^ba^ac 

1 

1 

l 

l 

0 

0 

Scc 

l 

1 

l 

0 

The  procedure  is  actually  applied  only  to  the  submatrix 


0 

« S"  ! 
aa  aa  ! 

! r 

i 

to  obtain 

Saa 

0 

0 

$ba  Saa  ! 

0 

SJb 

0 

The  resulting  square  root  covariance  matrix  is  given  in  upper  triangular 
form  by 


where  S 


ac 


$ S , Sl  = S"  , and  S+ 
aa  ac’  be  ba  ac’  cc 


cc 


To  illustrate  the  state  vector  permutation  procedure,  assume  the  state  vector 
prior  to  permutation  has  the  form 


S ! 
i ; 


' i 


and  the  square  root  covariance  Is  given  In  upper  triangular  form  by 


Saa 

Sab 

^ac 

0 

Sbb 

Sbc 

0 

0 

Scc 

Assume  further  that  the  state  vector  permutation  consists  of  an  Interchange 


of  x,  and  xK. 
a b 


Permutation  of  the  state  vector  variation  estimates  is  accomplished  by  simply 

A A 

interchanging  6x,  and  6xK.  Similarly  the  square  root  covariance  matrix  is 

d D 

permuted  by  a simple  row  interchange  to  obtain 


0 

S' 


bb 


be 


'aa  ab 
0 0 


ac 


L 


cc 


Since  the  row  interchange  destroys  the  triangularity  of  S,  the  retriangulari- 
zation  method  of  6.2  is  applied  to  the  submatrix 


’bb 


aa  ab 


to  obtain 


’bb 


ba 

+ 

aa 


Thus  the  permuted  and  retriangularized  S matrix  is  given  by 


r 


ute'  1 a1 1 TiWMAii 
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s 

i 

L 


*bb 

sba 

sbc 

0 

s« 

0 

0 

Kc 

where 


and  S 


+ 

cc 


6.8  General  Partitioned  Structure  of  the  State  Vector,  Transition 
Matrix,  and  Measurement  Sensitivity  Matrix 

Extensive  Use  of  vector  element  permutation  and  matrix  partitioning  has  been 
employed  throughout  this  report  for  the  purposes  of  illustrating  theoretical 
features  and  demonstrating  computationally  efficient  algorithms.  To  this 
point,  the  partitions  which  have  been  used  were  selected  on  a case  by  case 
basis  and  no  general  structure  has  been  evident.  The  purpose  of  this 
section  is  to  consolidate  the  piecemeal  use  of  permutation  and  partitioning 
into  a general  TRAM  structure. 


The  state  vector  may  be  augmented  with  additional  elements  when  an  RV  Is 
launched,  And  the  state  vector  elements  may  undergo  a permutation  when  the 
estimation  process  passes  from  one  trajectory  segment  to  another.  For  a 
given  processing  interval,  the  general  structure  of  the  state  vector  and 
transition  matrix  depends  on  whether  TMIG  data  is  used  to  construct  the 
trajectory  over  the  Interval  in  question.  The  structure  of  the  measurement 
sensitivity  matrix  depends  further  on  the  measurement  processing  option  which 
is  selected. 

When  TMIG  data  Is  used  to  construct  the  trajectory,  the  general  structure  of 
the  state  vector  is  as  follows. 


4 


■4-  ' ' ' 


78 


xfl  - trajectory  states  of  vehicle  on  segment  in  process  including  dynamic 
IMU  states 


xb  - trajectory  states  of  other  vehicles  which  have  been  augmented  including 
dynamic  IMU  states  of  those  vehicles  so  equipped 

xc  - metric  sensor  states  including  timing,  survey,  and  refraction  states* 

xd  - static  IMU  states,  including  timing,  of  other  vehicles  which  have 
been  augmented 

xfi  - IMU  timing  of  vehicle  on  segment  in  process 
xf  - static  IMU  states  of  vehicle  on  segment  in  process 
xg  - geopotential  states. 


The  corresponding  partitioned  structure  for  the  transition  matrix  is  given  by 


If  the  measurements  are  processed  asynchronously,  the  general  structure  of 
the  measurement  sensitivity  matrix  Is  given  by 


IK  i i K i i J 


However,  If  either  adjustable  estimation  time  processing  or  measurement 
variation  averaging  is  employed,  the  structure  is 


" ■ ?*r 


0 i 0 : &X+K*. 

: ax'  { ! 3x'  ax' 

i C i i e a 


. i 

ag 


When  a trajectory  segment  Is  constructed  without  the  use  of  7MIG  data,  the 
general  structure  of  the  state  vector  Is  given  by 


where 


x - trajectory  states  of  vehicle  on  segment  In  process 

x^  - trajectory  states  of  other  vehicles  which  have  been 
augmented  including  dynamic  IMU  states  of  those 
vehicles  so  equipped. 

x - metric  sensor  states  including  timing,  survey  and 
c 

refraction  states 

x.  - static  IMU  states  Including  timing,  of  all  augmented 
a 

vehicles  so  equipped 
x - geopotential  states. 


The  transition  matrix  for  this  case  has  the  structure 


If  the  measurements  are  processed  asynchronously. 
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But  if  adjustable  estimation  time  processing  or  measurement  variation 
averaging  is  employed,  then 
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6.9  Suboptimal  Processing  in  the  Presence  of  IMU  Noise 

In  the  development  of  the  navigation  variational  equations  in  Appendix  B,  it 
was  necessary  to  introduce  additional  states  to  account  for  the  noise  in  the 
IMU  velocity  output.  The  function  of  these  additional  states  is  to  model 
the  propagation  of  velocity  noise  into  position  and  velocity  variations. 

In  an  optimal  processor,  the  IMU  noise  states  are  included  in  the  state 
vector  and  are  estimated.  If,  however,  the  position  error  induced  by 
velocity  noise*  is  negligible  in  comparison  with  metric  sensor  position 
errors,  a suboptimal  processing  scheme  in  which  the  IMU  noise  states  are 
deleted  can  be  employed  with  only  slight  degradation  in  estimation  accuracy. 

The  principal  advantage  of  the  suboptimal  processor  is  that  computational 
efficiency  is  greatly  enhanced. 

♦For  MMIII,  this  error  is  estimated  to  be  less  than  one  foot  at  boost  termination. 
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Although  the  IMU  noise  states  are  deleted  and  the  effect  of  IMU  noise 
on  position  error  Is  neglected  in  the  suboptiraal  processor,  IMU  velocity 
noise  is  not  Ignored,  but  is  Instead  treated  as  an  equivalent  measurement 
error  in  each  of  the  metric  sensor  doppler  channels.  As  a consequence 
the  measurement  error  covariance  matrix  of  the  collective  doppler  channels 
is  modified  by  the  addition  of  another  matrix  to  represent  IMU  velocity 
noise.  Since  this  results  in  a nondiagonal  measurement  error  covariance, 
the  decorrelation  process  developed  in  6.2  must  be  applied  if  scalar 
measurement  update  fs  to  be  used. 


f 


In  order  to  achieve  a substantial  increase  in  computational  efficiency,  it 
is  necessary  to  employ  measurement  variation  averaging  in  conjunction  with 
the  suboptimal  processor.  (Note  that  this  is  made  possible  by  the  deletion 
of  the  IMU  noise  states,  since  this  eliminates  state  noise  in  the  navigation 
variation  equations.) 

An  algorithm  for  calculation  of  the  measurement  error  covariance  induced  by 
IMU  velocity  noise  is  developed  below. 


Let  D-| , ....  Dm  denote  the  doppler  channels  to  be  processed  on  an  interval 
over  which  the  measurement  variations  are  averaged.  Let  tQ,  ....  tN  denote 
the  discrete  readout  times  of  IMU  velocity  in  the  averaging  interval  (tQ, 
tN),  and  assume  the  IMU  velocity  noise  is  sequentially  uncorrelated. 


1.  For  each  i =1,  ....  M,  compute  the  retarded  measurement  times 

tjr  ...,  of  the  i-th  doppler  channel  which  are  in 

the  interval  (tQ,  t^). 

2.  For  each  i = 1,  ...»  M and  each  j = 1,  ...,  L(i),  determine 
k such  that  tk_-|  < tjj  < tk  and  set  C^.  equal  to  the 
3x3(N+l)  matrix 


/V!L lV  j/ Wi\ 

\ Wi  / 3 : V Wi  / 


3x3 (N+l) 


kth  block 


(k+l)th  block 
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3.  For  each  i =1,  ....  M,  put 

1 L(i) 

ci  * Hm  jj,  cu  • 

where  N(i)  is  the  number  of  measurements  in  the  averaging 
interval  from  the  i-th  doppler  channel. 

4.  For  each  i = 1,  ....  M,  evaluate  the  partial  derivative 
of  the  i-th  doppler  channel  measurement  with  respect  to 
velocity  at  the  mid  point  of  the  averaging  interval. 
Denote  this  by 

3DT 

3VT 

5.  Construct  the  Mx3(N+l)  matrix  £ given  by 


6.  Partition  Z into  Mx3  blocks,  i.e., 

f - Go  I - I V 

where  each  7^  is  Mx3. 

Now  denote  the  IMU  velocity  noise  sequence  by  rQ rN,  and  observe  that 

equivalent  measurement  error  in  the  doppler  channels  is  given  by 


i 
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Then  if  the  IMU  velocity  noise  covariance  is  given  by 
Cy  = Etr^r^]  , k * 0,  . . , N , 

it  follows  that  the  covariance  of  the  equivalent  doppler  measurement  error 
is  given  by 

AR  = Wo  + *“  + * 

The  total  doppler  measurement  error  covariance  is  then  formed  by  adding  AR 
to  the  doppler  measurement  error  covariance  due  to  receiver  noise. 


7.0 


PROGRAM  REQUIREMENTS 


The  purpose  of  this  section  Is  to  establish  guidelines  for  TRAM  program 
development.  These  guidelines  are  intended  to  insure  that  on  the  one  hand 
the  functional  requirements  of  estimation  and  error  analysis  are  satisfied, 
and  on  the  other,  the  program  structure  is  designed  to  be  sufficiently 
flexible  and  modular  to  provide  for  analysis  of  future  as  well  as  current 
types  of  test  vehicles  and  range  support  instrumentation. 

The  end-to-end  execution  of  the  TRAM  program  requires  essentially  three 
phases  of  operation,  consisting  of  respectively,  data  preparation,  estimation, 
and  error  analysis. 

In  the  data  preparation  or  pre-processing  phase,  metric  sensor  and 
telemetry  data  are  organized  into  files  suitable  for  use  in  the  estimation 
phase.  Included  in  this  phase  are  computation  of  refraction  errors  and 
their  partial  derivatives,  gross  editing,  and  statistical  analysis  of  raw 
metric  data.  Also  included  is  the  extraction  of  trajectory  information 
from  the  telemetry  data. 

In  the  estimation  phase,  the  recursive  filtering  and  smoothing  operations 
are  performed  iteratively  until  the  estimates  of  the  state  vector  elements 
converge. 

In  the  error  analysis  phase,  error  propagation  parameters  and  error  budget 
values  are  combined  to  obtain  estimates  of  the  mean  and  covariance  of  the 
state  vector  estimation  error. 

The  error  propagation  parameters,  required  in  the  error  analysis  phase,  can 
be  obtained  during  the  estimation  phase.  This  is  accomplished  by  augmenting 
the  filtering  and  smoothing  equations  to  include  the  sensitivity  of  the 
estimation  error  to  initialization  errors  (in  all  estimated  states  together 
with  a selected  subset  of  the  constrained  states)  and,  in  certain  cases, 
the  estimation  error  covariance  due  to  random  measurement  error. 

However,  the  option  should  exist  to  exercise  the  filtering  and  smoothing 
equations  in  an  error  propagation  mode  In  which  no  measurements  are  processed, 
but  in  which  all  error  propagation  parameters  are  obtained. 


In  the  sequel,  the  three  phases  of  TRAM  operation  will  be  considered  separately 
and  in  more  detail.  Functional  requirements,  program  modules  and  structure, 
user  options,  and  alternative  modes  of  operation  will  be  evident  from  the 
discussion. 

To  maintain  perspective,  only  those  program  operations  which  are  essential 
for  estimation  and  error  analysis  will  be  considered.  Auxiliary  operations 
required  to  provide  analyst  aids,  in  the  form  of  various  kinds  of  program 
output,  are  not  discussed.  The  reason  for  this  is  that  the  types  of 
auxiliary  operations  required  are  somewhat  dependent  on  the  user,  and  the 
capability  to  perform  these  operations  should  reside  in  separate  program 
modules  which  are  readily  augmented  or  modified. 

7.1  Pre-Processing  Phase 

The  purpose  of  the  pre-processing  phase  is  to  prepare  files  of  data  for  use 
in  the  estimation  phase.  The  particular  operations  which  must  be  performed 
are  highly  dependent  on  the  data  source.  However,  since  the  estimator 
performs  operations  of  filtering,  smoothing,  and  systematic  error  compensation, 
these  operations  should  not  be  performed  by  the  pre-processor,  on  data  to  be 
supplied  to  the  estimator,  regardless  of  the  data  source. 

7.1.1  Metric  Data  Pre-Processing 

The  required  pre-processing  functions  for  metric  data  are: 

(i)  gross  editing 
(ii)  refraction  calculations 
(ill)  statistical  analysis 
(iv)  file  organization 

The  gross  editing  function  serves  first  to  identify  intervals  over  which 
each  metric  station  is  tracking  properly.  Second  within  each  track  interval, 
subintervals  are  identified  over  which  each  sensor  channel  is  tracking. 

Finally,  isolated  points  are  identified  by  means  of  an  edit  flag,  where  loss 
of  data  or  transmission  error  occurs. 


In  order  to  avoid  aliasing  of  data,  the  level  at  which  isolated  points  are 
edited  must  be  well  above  the  level  of  systematic  and  random  measurement 
error.  Thus,  if  M is  a measurement,  R is  the  reference  used  for  editing, 
a is  the  rms  value  of  total  measurement  and  reference  error,  and  the  measure- 
ment is  edited  whenever 

|M  - R|  > L , 

then  L should  be  at  least  as  great  as  10  o. 

Refraction  calculations  are  required  in  the  pre-processor  in  order  to 
avoid  repeating  the  time  consuming  ray  trace  operations  on  each  iteration 
during  the  estimation  phase. 

The  refraction  parameters  are  computed  as  functions  of  geometric,  rather 
than  refracted,  range  and  elevation  angle.  These  parameters  are  required 
at  each  point  where  a range,  elevation  or  doppler  measurement  is  to  be 
processed.  Since  the  independent  variables,  i.e.,  geometric  range  and 
elevation  angle,  are  known  only  approximately  for  each  measurement,  the 
partial  derivatives  of  the  refraction  parameters  with  respect  to  the 
independent  variables  are  also  required. 

For  the  purpose  of  calculating  the  refraction  parameters  only,  the  raw 
measurements  should  be  corrected  to  obtain  the  best  a priori  value  of 
geometric  range  and  elevation  angle,  and  the  refraction  parameters  should 
be  evaluated  at  this  point. 

Furthermore,  when  ionospheric  refraction  is  significant,  both  group  and 
phase  refraction  parameters  must  be  computed  for  each  affected  measurement. 

The  statistical  analysis  of  the  metric  data  is  required  to  determine  the 
variances  due  to  random  measurement  error.  Unlike  the  measurement  error 
proper,  the  variances  are  smooth  functions  of  time.  Accurate  estimation 
of  measurement  error  variances  requires  smoothing  and  calculation  of  sample 
statistics  over  intervals  of  sufficient  duration  to  reduce  statistical  noise 
I to  a negligible  level. 
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When  both  sensor  element  outputs  and  encoder  outputs  are  included  In  the 
measurement  set  for  a given  channel,  the  variance  of  the  sensor  element 
measurement  noise  is  required  at  each  sensor  element  measurement  point. 

If  only  encoder  outputs  are  available,  the  variance  of  the  encoded  random 
error  is  required  at  each  encoder  measurement  point. 

The  raw  data,  refraction  parameters,  and  measurement  variances  are  organized 
into  separate  files  for  each  tracking  station.  A header  must  be  applied 
to  each  file  which  defines  the  tracking  Intervals  over  which  the  individual 
channels  of  the  station  maintain  track. 

The  data  in  each  file  Is  stored  sequentially,  in  the  order  of  Increasing 
time.  The  data  to  be  stored  Includes: 

encoder  data:  T,  R,  A,  E,  D 

2 2 2 2 

encoder  variances:  a^,  a A,  a^,  aD 

sensor  signals:  t,  r,  <|>,  i|>,  d 

2 2 2 2 

sensor  variances:  ar,  a a^,  od 

refraction  parameters:  p,  e,  R»E 

track  Indicator  flags 
edit  flags 
signal  strength 

7.1.2  Telemetry  Data  Pre-Processing 

The  pre-processing  of  telemetry  data  Involves  nothing  more  than  extraction 
of  trajectory  data  and  file  organization.  A file  header  must  be  generated 
which  defines  intervals  of  data  loss,  and  the  times  of  guidance  system 
Initialization,  staging  events,  vehicle  deployment,  and  system  shutdown. 

The  data  In  the  file  Is  stored  sequentially,  in  the  order  of  increasing 
time.  The  stored  data  includes: 

major  cycle  data:  time,  position,  velocity 

minor  cycle  data:  time,  accelerometer,  rate  gyro  and  platform  gimbal 
angle  outputs 


i 
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7.2 


Estimation  Phase 


The  operations  which  are  performed  in  connection  with  the  estimation  phase 
of  the  TRAM  program  can  be  categorized  as  follows: 

1.  Input 

2.  Initialization 

3.  Filter 

4.  Smoother 

5.  Convergence  Test 

6.  Reset 

Input  operations  include  those  which  must  be  performed  by  the  user  in  order 
to  initiate  the  estimation  phase. 

Initialization  operations  consist  of  those  which  must  be  performed  by  the 
program  before  any  filter  or  smoother  operations  can  occur. 

Filter  operations  consist  of  measurement  processing  and/or  error  propagation 
calculations.  Each  time  the  filter  is  activated,  it  performs  operations 
over  the  entire  trajectory.  The  filter  operations  are  recursive,  and 
proceed  in  the  direction  of  increasing  time  on  each  trajectory  segment. 

Each  segment  is  partitioned  into  intervals  over  which  the  filter  configuration 
is  fixed.  Thus  the  measurement  coverage  and  state  dynamics  do  not  change 
on  an  interval,  and  all  state  vector  augmentation  and  permutation  operations 
occur  at  interval  junctures.  Finally,  the  intervals  are  partitioned  into 
subintervals,  and  all  bulk  storage  I/O  operations  are  performed  at  sub- 
interval junctures. 

Smoother  operations  consist  of  filter  output  processing  and/or  error 
propagation  calculations.  The  smoother  is  activated  at  the  conclusion  of 
each  set  of  filter  operations  over  the  entire  trajectory.  The  smoother 
operations,  once  activated,  are  also  performed  recursively  over  the  entire 
trajectory,  but  in  precisely  the  reverse  order  to  those  of  the  filter  and 
in  the  direction  of  decreasing  time*. 

♦There  is  one  exception  to  this  which  will  be  explained  later. 
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A convergence  test  is  performed  at  the  conclusion  of  each  set  of  smoother 
operations  over  the  entire  trajectory.  This  test  determines  if  the  state 
vector  estimates  have  converged  or  if  the  maximum  number  of  iterations  has 
occurred.  If  neither  part  of  the  test  is  satisfied,  the  program  performs 
another  iteration  of  the  filter  and  smoother  operations. 

The  reset  operation  is  performed  whenever  an  iteration  of  the  filter  and 
smoother  operations  is  required.  The  reset  operation  is  required  to 
reinitialize  the  filter  state  vector  and  square  root  covariance  to  the 
original  input  values.  Also  the  nominal  state  vector  is  equated  to  the 
estimated  state  vector  obtained  by  the  preceding  smoothing  operation. 

The  requirements  for  each  of  the  above  operational  categories  will  now  be 
considered. 

7.2.1  Input  Requirements 

A.  Schedules 

The  user  must  define  the  order  in  which  the  trajectory  segments  are  to  be 
processed.  A measurement  schedule  for  each  tracking  station,  a schedule 
indicating  portions  of  the  trajectory  which  are  inertial ly  instrumented, 
and  a schedule  indicating  thrust  termination  and  reentry  points  on  the 
trajectory.  (Some  of  these  schedule  items  are  derivable  from  the  file 
headers  generated  in  the  pre-processing  phase.) 

The  schedule  inputs  are  required  in  order  to  develop  processing  intervals 
over  which  the  filter  configuration  is  invariant. 

B.  Parameters 

The  user  must  specify  the  complete  set  of  parameters  to  be  employed  in  the 
program.  These  include  a priori  estimates  of  all  state  vector  elements  and 
the  state  covariance  matrix.  Parameters  which  are  not  included  in  the  state 
vector  but  which  enter  either  the  state  or  measurement  equations  must  be 
specified.  Also,  the  state  noise  covariance  must  be  specified,  and  if  a 
measurement  covariance  Is  to  be  used  which  differs  from  that  implied  by  the 
measurement  variances  included  in  the  data  files,  it  must  also  be  specified. 
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C.  State  Vector  Categorization 


i 

| 

The  user  must  specify,  for  each  state  vector  element,  whether  it  is  to  be  ! 

estimated  or  constrained.  From  the  subset  of  constrained  elements,  the 
user  must  specify  those  which  are  to  be  propagated  for  error  analysis. 

D.  Measurement  Processing 

For  each  filter  processing  interval,  the  user  must  specify  the  type  of 

measurement  processing  to  be  used.  The  options  are:  j 

1 . asynchronous  ] 

i - 

5 i 

2.  adjusted  estimation  time  j; 

|| 

3.  measurement  variation  averaging  jj 

r 

The  first  may  be  used  in  any  case.  The  latter  two  are  recommended  only 
when  state  noise  can  safely  be  ignored. 

When  the  adjusted  estimation  time  option  is  to  be  employed,  the  user  must 
specify  the  estimation  times. 

When  the  measurement  variation  averaging  option  is  to  be  employed,  the 
user  must  specify  the  intervals  (within  the  filter  processing  subintervals) 
over  which  the  averaging  is  performed  and  the  estimation  time  within  each 
averaging  interval. 

In  addition  to  the  above,  the  user  must  also  indicate  whether  the  processing 
is  to  be  restricted  to  the  use  of  encoder  outputs.  If  this  is  to  be  the 
case,  the  user  must  specify  the  dynamic  error  coefficients  to  be  used  for 
each  sensor. 

E.  Nominal  State  Vector 

For  the  initial  set  of  filter  operations,  the  user  must  specify  the  nominal 
state  vector.  (On  subsequent  iterations  the  nominal  state  vector  is 
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automatically  set  by  the  program.)  The  user  must  always  specify  the  initial 
condition  for  the  nominal  state  vector,  but  he  also  has  the  option  of 
specifying  the  nominal  state  vector  at  arbitrary  times  over  the  entire 
trajectory.  When  the  user  specifies  only  the  initial  condition  of  the  nominal 
state  vector,  he  has  the  option  of  allowing  the  program  to  reset  the 
nominal  state  vector  at  arbitrary  times  by  equating  it  to  the  filter 
estimate  of  the  state  vector. 

It  should  be  noted,  that  although  the  user  can  In  effect  specify  nominal 
state  vector  resets  arbitrarily,  the  program  will  actually  restrict  the 
occurrence  of  resets  to  certain  points  within  the  filter  cycle. 

F.  Convergence  Criteria 

The  user  must  specify  the  convergence  criteria  of  the  estimator.  The  user 
must  also  specify  the  maximum  number  of  iterations  of  the  filter/ smoother 
which  are  to  be  allowed  in  attempting  to  satisfy  the  convergence  test. 

The  convergence  criteria  will  be  based  on  the  differences  between  elements 
of  the  estimated  states  obtained  on  successive  iterations.  For  each  estimated 
state  element,  a test  of  the  form  "Is  |x^+1^  - x^ < ek?"  will  be  applied, 
where  1 denotes  the  iteration  count,  k denotes  the  state  vector  element,  and 
ek  > 0 is  specified  by  the  user. 

7.2.2  Initialization  Requirements 

Initialization  operations  consist  of  those  which  are  performed  prior  to 
the  first  set  of  filter/ smoother  operations  and  which  are  not  repeated 
on  subsequent  iterations. 

A.  Control  Logic 

The  operations  to  be  performed  by  the  filter  and  smoother  are  dependent  on 
user  inputs  and  program  constraints.  The  control  of  these  operations  can 
be  accomplished  by  the  use  of  various  logical  variables  (flags)  and  the 
specification  of  times  at  which  discrete  transitions  in  processing  are  to 
occur. 
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Each  state  vector  element  has  a flag  to  Indicate  in  which  of  three  distinct 
categories  it  falls: 


1 . Estimated 

2.  Propagated 

3.  Constant 

The  constant  states  are  invariant  throughout  all  program  operations.  The 
propagated  states  are  constant  for  all  program  operations  other  than  those 
involving  numerical  partial  derivative  calculations  with  respect  to  these 
states  as  required  for  error  propagation. 

The  estimator  state  vector  configuration  is  fixed  on  each  processing  interval, 
and  includes  all  estimated  states  which  have  been  augmented  up  to  and  including 
the  epoch  of  the  interval  In  process.  A permutation  array,  or  its  logical 
equivalent,  must  be  generated  for  each  processing  interval  to  indicate  (i) 
which  elements  are  in  the  estimated  state  vector  and  (il)  the  order  of  these 
elements  within  the  estimated  state  vector. 


The  beginning  and  ending  times  for  each  processing  Interval  must  be  set,  and 
flags  must  be  generated  to  indicate  the  measurements  to  be  processed.  The 
specification  of  measurements  Includes  the  designation  of  tracking  stations 
and  the  sensor  channels  to  be  processed. 

Flags  to  indicate  the  type  of  measurement  processing  and  the  type  of  smoother 
to  be  used  on  each  interval  must  also  be  set. 

Finally  within  each  processing  Interval,  the  beginning  and  ending  times  for 
the  I/O  subintervals  must  be  set. 

At  the  juncture  of  processing  Intervals,  augmentation  and/or  permutation  of 
the  elements  of  the  estimated  state  vector  can  occur.  This  requires  discrete 
processing  of  the  estimator  state  vector  and  square  root  covariance  matrix. 
The  logic  to  control  these  discrete  operations  must  be  set  during 
Initialization. 
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On  the  first  filtering  pass  only,  the  nominal  state  vector  Is  reset  at 
arbitrary  times  which  have  been  designated  during  initialization. 

Filter  logic  will  Inhibit  nominal  state  vector  reset  except  at  designated 
points  within  its  cycle,  and  a reset  will  actually  occur  at  the  first  of 
these  poihts  to  be  reached  after  the  designated  reset  time. 

As  an  alternative  to  the  specification  of  arbitrary  reset  times,  flags  must 
be  available  which  provide  for  either  of  two  extreme  cases: 

(1)  no  reset 

(ii)  reset  at  the  highest  possible  rate,  i.e. , at  every  allowable 
point  of  reset  in  the  filter  cycle. 

A flag  must  also  be  set  to  indicate  whether  the  nominal  state  vector  is  to 
be  reset  externally  or  internally.  In  the  former  instance,  the  reset  is 
accomplished  by  interpolation  of  the  external  reference  to  the  actual  reset 
time.  In  the  latter  case,  the  reset  is  accomplished  by  equating  the 
nominal  state  vector  to  the  value  obtained  from  the  filter  estimate  at  the 
actual  reset  time.  (In  either  case,  the  state  variation  estimate  must  be 
reset  such  that  the  whole  state  vector  estimate  at  reset  is  invariant.) 

Finally,  flags  must  be  set  to  indicate  whether  error  propagation  calculations 
are  to  be  performed  during  the  estimation  phase  or  postponed  until  the  error 
analysis  phase,  and  to  control  the  optional  calculation  associated  wicn 
error  propagation. 

B.  Calculation  of  Constants 

Depending  on  the  assigned  category  for  each  state,  a number  of  program 
variables  may  actually  be  constant  during  either  the  estimation  phase  or 
the  error  analysis  phase,  or  both.  For  example,  station  coordinates, 
coordinate  transformations,  geopotential  parameters,  and  so  on,  may  be 
constant  during  an  entire  phase  of  program  operation. 

It  is  important  to  compute,  to  the  extent  possible,  all  program  constants 
during  initialization.  Furthermore,  the  structure  of- the  routines  which 
are  used  in  the  TRAM  program  should  be  designed  so  that  the  flags  which 
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define  the  category  of  each  state  element  can  be  used  to  bypass  program 
blocks  whenever  these  blocks  serve  only  to  recompute  constants. 

C.  Filter  Initialization 

Prior  to  filter  operation,  the  filter  nominal  state  vector,  state  vector 
variation  estimate,  and  square  root  covariance  must  all  be  initialized. 

The  filter  nominal  state  vector  Is  equated  to  the  Initial  condition  which 
the  user  has  specified  for  the  nominal  state  vector.  However,  the  initial 
condition  of  the  nominal  state  vector  must  be  brought  into  time  commensuration 
with  the  a priori  state  vector  estimate,  if  this  condition  is  not  already 
satisfied.  This  Is  accomplished  by  Interpolation  if  possible;  otherwise  it 
is  accomplished  by  integration  of  the  state  differential  equation  using 
the  nominal  state  vector  Initial  condition. 

The  state  vector  variation  estimate  Is  equated  to  the  difference  (estimate 
minus  nominal)  between  the'a  priori  state  vector  estimate  and  the  initial 
value  of  the  nominal  state  vector. 

The  square  root  covariance  is  Initialized  by  applying  the  Cholesky 
decomposition  to  the  a priori  covariance  of  the  state  vector  estimation 
error.  The  object  matrix  to  which  the  Cholesky  decomposition  is  applied  is 
obtained  by  row/column  permutations  of  the  a priori  covariance  to  conform 
with  the  initial  state  vector  configuration. 

7.2.3  Filter  Requirements 

At  Interval  junctures,  elements  may  be  augmented  to  the  filter  state  vector, 
and  the  filter  state  vector  may  then  undergo  a permutation.  Correspondingly, 
augmentation  and  permutation  operations  must  be  performed  on  the  square  root 
covariance  matrix  of  the  filter,  which  must  also  be  retriangularized. 

All  filter  I/O  operations  are  performed  at  subinterval  junctures.  The 
inputs  include  measurement  data  and  the  nominal  trajectory.  The  required 
outputs  are  dependent  on  the  particular  processing  options  In  effect,  but 
basically  Include  all  filter  outputs  and  error  propagation  parameters  required 
by  the  smoother. 
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Within  each  subinterval  the  filter  operates  in  a cyclical  manner  performing 
functions  of  extrapolation,  update,  error  propagation,  and  nominal  resets 
when  required.  The  specific  filter  requirements  will  be  considered 
separately  for  each  of  the  measurement  processing  options. 

A.  Asynchronous  Processing 

Asynchronous  measurement  processing  is  recommended  on  each  interval  over 
which  state  noise  is  not  neglibible,  but  it  may  in  fact  be  used  on  any 
interval . 

Consider  an  interval  on  which  asynchronous  measurement  processing  is  to 
be  employed.  On  this  interval,  the  measurements  from  a fixed  set  of 
tracking  stations  are  scheduled  for  processing. 

The  processing  is  performed  recursively  on  blocks  of  measurements,  each 
consisting  of  one  measurement  from  each  scheduled  sensor  channel  of  each 
scheduled  tracking  station  on  the  interval  under  consideration. 

The  first  step  in  the  processing  of  a block  is  to  check  the  edit  flags 
and  delete  those  channels  which  have  been  edited  during  the  pre-processing 
phase. 

Next,  for  each  measurement  that  remains  in  the  block,  the  nominal  measurement 
is  computed  along  with  the  nominal  receive  time  and  retarded  time  of  the 
measurements  at  each  station,  and  measurement  variations  are  formed. 

The  measurement  variations  of  each  station  are  then  processed  in  turn, 
where  the  order  of  station  processing  is  the  same  as  the  order  of  increasing 
retarded  measurement  times. 

For  each  station,  the  processing  consists  of  extrapolation  operations  followed 
by  update  operations.  The  extrapolation  operations  are  performed  to  bring 
the  filter  and  error  propagation  parameters  up  to  the  station  retarded 
measurement  time.  Then,  filter  and  error  propagation  update  operations  are 
performed  fcr  each  sensor  channel  of  the  station. 
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If  nominal  state  vector  resets  have  been  scheduled  on  the  first  filter  pass, 
they  only  can  occur  Immediately  following  the  processing  of  one  measurement 
block,  prior  to  computing  the  nominal  retarded  measurement  times  of  the 
next  block.  Thus  at  the  conclusion  of  the  processing  of  each  measurement 
block,  on  the  first  filter  pass  only,  a test  is  performed  to  determine  if 
a scheduled  reset  time  has  been  passed  during  the  processing  of  that  block. 
If  so,  the  nominal  state  vector  and  state  variation  estimate  are  reset  at 
the  time  corresponding  to  the  end  of  the  block. 

On  subsequent  filter  passes,  nominal  state  vector  resets  do  not  occur, 
since  the  nominal  state  vector  is  always  equal  to  the  whole  state  estimate 
obtained  on  the  preceding  smoother  pass. 

However,  when  fixed-interval  smoothing  is  employed  an  operation  which 
resembles  reset  must  be  performed  as  a part  of  each  extrapolation.  This 
operation  is  required  because  the  variation  estimates  are  extrapolated 

y 

with  state  noise  equal  to  zero. 

Thus  when  fixed- interval  smoothing  is  employed,  the  nominal  state  vector 
and  the  state  variation  estimate  are  each  extrapolated  (to  the  retarded 
measurement  time  of  the  station  in  process)  with  state  noise  equal  to  zero. 
Then  the  difference  between  the  extrapolated  state  vector  and  the  nominal 
state  vector,  interpolated  to  the  retarded  time,  is  computed  (extrapolated 
minus  nominal),  and  the  difference  is  added  to  the  extrapolated  state 
variation  estimate  to  obtain  an  adjusted  state  variation  estimate  at  the 
retarded  measurement  time.  The  adjusted  state  variation  estimate  is  used 
in  subsequent  processing,  and  the  extrapolated  state  vector  and  extrapolated 
state  variation  estimate  have  no  further  use. 

Intermediate  filter  outputs  are  required  on  processing  intervals  in  which 
fixed-interval  smoothing  is  to  be  employed.  In  this  case  filter  outputs 
must  be  saved  before  and  after  the  update  operations  for  each  station  in 
every  processing  block. 

Thus,  within  each  processing  block,  when  the  extrapolation  operations  for 
a given  station  are  complete,  the  nominal  state  vector  and  state  variation 
estimate  along  with  the  nominal  received  and  retarded  measurement  times. 
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the  filter  square  root  covariance  matrix,  the  filter  transition  matrix, 
and  the  error  propagation  parameters  must  be  saved.  Then  when  the  update 
operations  for  all  sensor  channels  of  the  station  are  complete,  the  state 
variation  estimate,  the  filter  square  root  covariance  matrix,  and  the  error 
propagation  parameters  must  again  be  saved. 

B.  Processing  with  Adjustable  Estimation  Time 

Adjustable  estimation  time  processing  is  recommended  only  when  state  noise 
is  negligible*. 

Adjustable  estimation  time  processing  is  identical  with  asynchronous 
processing  to  the  point  where  the  measurement  variation,  receive  time, 
and  retarded  time  have  been  computed  for  each  measurement  in  a block. 

At  this  point,  extrapolation  operations  are  performed  to  bring  the  filter 
and  error  propagation  parameters  up  to  the  estimation  time  which  has  been 
designated  for  the  measurement  block  as  a whole. 

Then,  filter  and  error  propagation  update  operations  are  performed  on  the 
entire  block  of  measurement  variations  in  any  convenient  order. 

If  nominal  state  vector  resets  have  been  scheduled  on  the  first  filter  pass, 
they  can  only  occur  imnediately  following  the  completion  of  the  set  of  update 
operations  for  an  entire  measurement  block.  Thus,  whenever,  on  the  first 
filter  pass  only,  a scheduled  reset  time  has  been  passed  during  the  processing 
of  a measurement  block,  the  nominal  state  vector  and  the  state  variation 
estimate  are  reset  at  the  estimation  time  associated  with  the  block. 

On  subsequent  filter  passes,  nominal  state  vector  resets  do  not  occur. 

However,  when  fixed-interval  smoothing  is  employed,  the  same  reset-like 
operation  described  for  asynchronous  processing  must  be  performed  at  the 
close  of  each  extrapolation. 


♦Although  the  actual  state  noise  process  may  be  negligible,  this  does  not 
preclude  the  use  of  a nonzero  state  noise  covariance  matrix  in  the  filter 
mechanization  and  subsequent  use  of  the  fixed-interval  smoother.  This  type 
of  mechanization  can  be  useful  in  reducing  unmodeled  error  growth  in  the 
estimates. 
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On  processing  Intervals  in  which  fixed- interval  smoothing  is  to  be  employed, 
filter  outputs  must  be  saved  before  and  after  the  set  of  update  operations 
for  each  measurement  block. 


Thus  when  the  extrapolation  operation  for  a measurement  blocx  is  complete, 
the  nominal  state  vector,  the  state  variation  estimate,  the  designated 
estimation  time,  the  filter  square  root  covariance  matrix,  the  filter 
transition  matrix,  and  the  error  propagation  parameters  must  be  saved. 

Then,  when  the  set  of  update  operations  for  the  block  as  a whole  is  complete, 
the  state  variation  estimate,  trie  filter  square  root  covariance  matrix, 
and  the  error  propagation  parameters  must  be  saved. 

C.  Processing  with  Measurement  Variation  Averaging 

Processing  with  measurement  variation  averaging  is  an  extension  of 
adjustable  estimation  time  processing,  and  is  likewise  recommended  only 
when  state  noise  is  negligible. 

When  measurement  variation  averaging  is  employed,  each  processing  subinterval 
is  partitioned  into  averaging  intervals.  Then,  for  each  averaging  interval, 
a block  of  measurements  is  formed,  and  an  estimation  time  within  the  interval 
is  designated. 

The  measurements  which  constitute  a block  consist  of  all  measurements,  within 
the  averaging  interval  of  the  block,  from  all  scheduled  tracking  stations 
for  the  interval  in  process. 

Once  a block  is  'ormed,  edit  flags  are  tested,  and  individual  measurements 
which  have  been  edited  during  the  pre-processing  phase  are  deleted  from 
the  block.  Then,  for  each  measurement  remaining  in  the  block,  calculation 
of  the  nominal  measurement  and  nominal  receive  and  retarded  measurement 
times  is  performed. 

Next  the  measurement  variations  for  the  block  as  whole  are  computed  and, 
for  each  sensor  channel,  the  average  measurement  variation  over  the 
averaging  interval  is  computed. 


99 


| Extrapolation  operations  are  now  performed  to  bring  the  filter  and  error 

i propagatipn  parameters  up  to  the  estimation  time  which  has  been  designated 

i 

for  the  averaging  interval. 

After  extrapolation,  the  filter  and  error  propagation  update  operations  are 
performed  on  the  entire  block  of  averaged  measurement  variations  (consisting 
of  at  most  one  per  sensor  channel)  in  any  convenient  order. 

If  nominal  state  vector  resets  have  been  scheduled  on  the  first  filter  pass, 
they  can  only  occur  following  the  completion  of  the  set  of  update  operations 
for  an  entire  averaging  interval.  Thus,  on  the  first  filter  pass  only,  at 
the  conclusion  of  the  set  of  update  operations,  a test  is  performed  to 
determine  whether  a nominal  reset  has  been  scheduled  at  any  time  within  the 
averaging  interval. 

If  no  nominal  reset  has  been  scheduled  within  an  averaging  interval,  the 
filter  processing  described  above  is  repeated  for  the  next  block  of  measure- 
ments and  the  next  averaging  interval. 

But  if  a nominal  reset  has  been  scheduled  within  the  averaging  interval,  it 
is  now  performed.  Moreover,  regardless  of  the  designated  reset  time,  the 
j.  actual  reset  takes  place  at  the  estimation  time  of  the  interval.  If  the 

[ nominal  is  to  be  reset  from  an  external  source,  the  source  value  is  first 

interpolated  or  integrated  to  the  interval  estimation  time.  Then  the 
nominal  state  vector  and  the  state  variation  estimate  are  reset,  and  the 
filter  processing  described  above  is  repeated  for  the  next  block  measure- 
ments on  the  next  averaging  interval. 

On  processing  intervals  in  which  fixed-interval  smoothing  is  to  be  employed, 
filter  outputs  must  be  saved  before  and  after  the  set  of  update  operations 
for  each  averaging  interval. 

Thus  when  the  extrapolation  operation  for  an  averaging  interval  is  complete, 
the  nominal  state  vector,  the  state  variation  estimate,  the  designated 
estimation  time,  the  filter  square  root  covariance  matrix,  the  filter 
transition  matrix  (from  the  preceding  estimation  time  to  the  current 
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estimation  time),  and  the  error  propagation  parameters  must  be  saved.  Then, 
when  the  set  of  update  operations  for  the  averaging  Interval  Is  complete, 
the  state  variation  estimate,  the  filter  square  root  covariance  matrix,  and 
the  error  propagation  parameters  must  be  saved. 

As  in  all  other  cases,  between  the  points  at  which  filter  outputs  before 
and  after  update  are  saved  for  the  smoother,  no  nominal  reset  Is  allowed. 

7.2.4  Smoother  Requirements  | 

i 

The  two  smoother  types  which  may  be  employed  in  the  TRAM  program  differ  j 

substantially  in  mechanization  and  operation,  and,  for  this  reason,  the  ; 

two  will  first  be  considered  separately.  j 

j 

A.  Retrograde- Integration  Smoother 

This  smoother  is  by  far  the  simpler  of  two  types,  but  it  can  be  effectively 
employed  only  when  state  noise  is  negligible. 

On  intervals  in  which  retrograde-integration  smoothing  is  to  be  employed, 
it  is  only  necessary  to  integrate  the  dynamic  states  and  the  error 
propagation  parameters.  The  integration  process  is  initialized  at  the 
end  of  the  interval  and  proceeds  in  the  direction  of  decreasing  time. 

Static  states  are  held  fixed  during  each  retrograde-integration. 

I 

This  type  of  smoother  requires  no  intermediate  filter  output,  but  its  own  j 

output  can  be  stored  at  the  same  subinterval  junctures  as  used  for  filter  j 

inputs.  ! 

If  the  retrograde-integration  smoother  Is  used  on  an  interval  at  the  end 
of  a trajectory  segment,  then  it  is  initialized  by  the  final  filter  estimate 
of  the  whole  state  on  that  interval.  When  this  case  occurs,  the  final  time 
on  the  interval,  i.e..  Initial  time  for  the  retrograde- integrator , must  be 
adjusted  by  applying  to  the  nominal  vehicle  time  tag  at  the  end  of  the 
interval,  the  difference  between  the  filter  estimate  of  and  the  nominal 
value  of  the  vehicle  timing  correction  at  the  end  of  the  interval. 
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6.  Fixed* Interval  Smoother 


The  fixed- Interval  smoother  Is  employed  on  processing  Intervals  In  which 
state  noise  Is  not  negligible.  It  may  also  be  used  at  Interval  junctures, 
coinciding  with  vehicle  deployment,  where  random  separation  errors  occur. 

The  fixed- Interval  smoother  operates  recursively  on  filter  outputs  and 
error  propagation  parameters,  but  In  reverse  order  to  the  filter  recursion. 
The  smoother  requires  the  filter  estimates  of  the  whole  state  vector  before 
and  after  each  filter  update.  These  are  obtained,  respectively,  by 
combining  the  nominal  state  vector  and  the  filter  estimates  of  the  state 
variation  before  and  after  each  update.  The  subinterval  structure  used 
for  filter  I/O  operations  is  also  used  for  smoother  I/O. 

At  each  stage  in  fixed- interval  smoothing,  the  time  tag  of  the  filter 
estimate  must  be  adjusted  by  applying  the  difference  between  the  smoother 
estimate  and  the  nominal  value  of  the  vehicle  timing  correction  at  the 
estimation  time. 


At  the  conclusion  of  the  entire  smoothing  process,  the  smoothed  estimates 
can  be  interpolated  to  any  convenient  set  of  times,  designated  by  the  user, 
and  stored  for  future  use.  For  this  It  is  only  necessary  that  the  stored 
smoothed  estimates  be  sufficiently  closely  spaced  such  that  smoothed 
estimates  at  arbitrary  times  can  be  accurately  obtained  using  low  order 
interpolating  splines. 

C.  Comments  on  the  Smoothing  Process  for  the  Trajectory  as  a Whole 

The  smoothing  operation  is  performed  over  the  trajectory  as  a whole  in 
exactly  the  reverse  order  to  the  filtering  operations.  At  Interval  junctures 
the  smoother  may  change  from  one  type  to  another,  but  the  process  is 
essentially  continuous.  Furthermore,  when  discontinuities  occur  at  interval 
juncture  such  as  with  separation  errors  at  deployment,  the  fixed- Interval 
smoother  Is  used  to  make  the  transition,  regardless  of  the  smoother  types 
used  In  either  of  the  Intervals  forming  the  juncture. 


\ 
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It  was  mentioned  earlier  that  there  is  one  exception  to  the  rule  that  the 
smoother  operates  in  reverse  order  to  the  filter.  The  single  exception 
occurs,  as  a processing  option,  when  state  noise  is  negligible  in  the 
inertial  guidance  system  of  the  boost  vehicle. 

In  this  case,  at  the  conclusion  of  the  filter  pass  on  any  iteration,  the 
initial  conditions  for  the  smoother  estimate  of  the  boost  vehicle  trajectory 
are  completely  known,  and  since  there  is  no  state  noise,  the  smoothed  boost 
vehicle  trajectory  can  be  reconstructed  by  forward  integration  of  the  navigation 
equations.  (It  is  tacitly  assumed  that  the  launch  point  survey  error  has  been 
estimated,  or  it  is  negligible,  in  which  case  the  uncertainty  in  the  navigation 
initial  position  and  velocity  is  zero.) 

When  this  option  is  exercised  on  the  boost  segment,  the  smoother  processing 
on  all  other  segments  is  unaffected,  except  that  it  terminates  at  points  just 
after  vehicle  deployment,  and  it  is  no  longer  necessary  to  smooth  the  dis- 
continuity at  deployment  events. 

7.2.5  Convergence  Test 
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The  number  of  filter/ smoother  iterations  which  are  performed  during  the  ’ 

estimation  phase  is  determined  by  the  convergence  test  which  is  performed 
at  the  end  of  each  iteration. 

The  first  part  of  the  test  is  performed  by  comparing  differences,  in  each 
of  the  components  of  the  estimated  state  vector,  obtained  on  successive 
iterations.  For  convergence,  each  element  of  the  estimated  state  vector 
must  satisfy  the  convergence  criterion  which  has  been  specified  by  the 
user.  This  part  of  the  test  is  performed  on  the  second  and  all  subsequent 
estimation  cycles. 

i 

The  other  part  of  the  test  simply  counts  the  number  of  estimation  cycles  ' -J 

which  have  been  performed  and  compares  this  with  the  maximum  number  the 

user  has  allowed  „ j 
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If  the  maximum  number  of  cycles  permitted  by  the  user  Is  attained  without 
satisfaction  of  the  convergence  criteria,  program  operation  is  halted  and 
an  error  message  to  this  effect  is  generated. 

7.2.6  Reset 

When  the  convergence  test  results  in  another  Iteration  of  the  estimator,  it 
is  necessary  to  perform  a reset  operation. 

The  reset  is  required  to  set  the  nominal  state  vector  and  reinitialize  the 
filter. 

The  nominal  state  vector  is  equated  to  the  estimated  state  vector  obtained  by 
the  smoother.  For  this  purpose,  the  smoother  trajectory  estimates  can  be 
Interpolated  to  a designated  set  of  time  points.  Then,  for  intermediate 
points  required  on  the  next  filter  cycle,  spline  interpolation  car  be  used 
between  the  designated  time  points. 

Reinitialization  is  required  for  the  filter  state  variation  estimate  and 
the  filter  square  root  covariance  matrix.  The  state  variation  estimate  is 
equated  to  the  difference  between  the  a priori  whole  state  vector  estimate 
and  the  value  of  the  new  nominal  state  vector  at  initialization  time. 

7.3  Error  Analysis  Phase 

In  this  phase  of  orocessing  an  error  analysis  of  the  estimation  process  is 
performed.  This  is  accomplished  by  processing  an  error  budget,  specified 
by  the  user,  with  the  error  propagation  parameters  of  the  estimation  process. 
The  result  of  the  error  analysis  Is  the  bias  and  covariance  of  estimation 
error  based  on  the  specified  error  budget. 

In  Its  complete  form  the  error  budget  must  include  a schedule  of  state  and 
measurement  noise  covariances  as  well  as  the  mean  and  covariance  of  the 
initial  value  of  the  vector  whose  elements  are  the  estimated  and  propagated 
states. 
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The  error  propagation  parameters  may  be  obtained  during  the  estimation  phase. 
However,  it  is  usually  more  efficient  to  defer  calculation  of  tiese  parameters 
until  the  estimation  process  converges.  Also,  it  may  be  required  for  planning 
purposes  to  perform  an  error  analysis  in  advance  of  the  estimatior  phase. 

For  these  reasons,  an  error  propagation  mode  is  required  in  which  the  error 
propagation  parameters  are  obtained  separately  from  the  estimation  phase. 

In  the  error  propagation  mode,  the  nominal  state  vector  is  supplied  externally, 
and  it  is  necessary  to  perform  all  estimator  functions,  except  measurement 
processing,  as  well  as  the  error  propagation  functions.  In  the  particular 
case  in  which  the  error  propagation  mode  follows  the  estimation  phase,  the 
externally  supplied  nominal  state  vector  is  equal  to  the  whole  state  vector 
estimate  obtained  during  the  estimation  phase. 

In  order  that  the  noise  induced  errors  be  properly  accounted  far,  the  filter 
and  smoother  gains  actually  used  for  estimation  must,  with  one  exception,  be 
based  on  the  noise  covariance  schedules  in  the  error  budget.  The  one  ex- 
ception occurs  on  intervals  in  which  state  noise  is  negligible,  in  which  case 
the  covariance  of  estimation  error  due  to  measurement  noise  can  be  computed 
by  the  alternate  method  given  in  Section  5.0,  regardless  of  how  the  filter 
gains  are  obtained. 
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A.  LINEAR  ESTIMATION 

Consider  a linear  stochastic  system  described  by 

(1)  ^1+1  * $i^i  + Uj , 1 * 0|  1»  N-l  i 

(2)  y»  * + v.,  1 = 0,  1,  • ••*  N . 

In  these  equations  x Is  the  state  vector,  and  y is  the  measurement  vector, 
(u^ , i =0,  1,  ....  N-l}  is  a sequence  of  random  vectors  called  the  state 
noise  process,  and  (v^,  i = 0,  1,  ....  N)  is  a similar  sequence  called  the 
measurement  noise  process.  These  processes  are  assumed  to  be  zero  mean, 
sequentially  uncorrelated,  and  mutually  uncorrelated  with  each  other  and  xQ. 
Mathematically  these  assumptions  are  expressed  as  follows: 

E(u^ ) = 0,  E(u.,.uT)  * QfS-jji  J = 0,  1,  ....  N-l  , 

E(vi)  = °*  E<Vj)  = Ri6ij;  i = 0,1 N 


E (ui vj ) = °»  E^xoui^  = °»  E<vJ>  = 0 ; 


where 


6. . = 


f 1.  i ■ J 
I 0,  1 ^ j 


Q and  R are  the  state  noise  and  measurement  noise  covariance  matrices,  re- 
spectively. To  complete  the  system  description,  it  is  assumed  that  the 

A 

a priori  mean  and  covariance  of  xQ,  denoted  by  x^  and  P^,  respectively,  are 
also  specified. 

The  estimation  procedures  to  be  considered  in  the  remainder  of  tiis  appendix 
will  apply  to  the  linear  system  given  by  (1)  and  (2). 
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A.l  Optimal  Linear  Estimation 


For  the  system  (1),  (2),  let  x(i|j)  denote  a function  which  is  in  the  form 
of  a constant  plus  a linear  function  of  the  measurement  set  y , ....  y.  and 
which  has  the  property  E[x(i|j)]  = E(x^).  Such  a function  is  said  to  be  a 
linear  unbiased  estimate  of  x.  given  y » ...»  y.. 

1 ^ J 

A 

Let  x(i|j)  be  a linear  unbiased  estimate  of  x.  given  y , ....  y.,  and  suppose 

A I V J 

x(i|j)  has  the  property* 

Etllxd'i)  - *il|2]  < E[||i(1|j)  - x,||2] 
for  all  linear  unbiased  estimates  x(i|j)  of  x,  given  y , ....  y..  Then 

A I O J 

x(i|j)  is  said  to  be  an  optimal  linear  estimate  of  x,  given  y , ....  y.. 

• V/  J 

It  can  be  shown  that  an  optimal  linear  estimate  of  x^  given  yQ,  ....  y^ 
always  exists  and  Is  unique.  The  notation  x(i|j)  will  be  used  exclusively 
to  denote  the  optimal  estimate  defined  above,  and  the  notation  P(i|j)  will 

A 

be  used  to  denote  the  error  covariance  of  x(i|j)  defined  by 
F(1|J)  5 E[(i(f|J)  - x,)  (x(1|j)  - x,)T]  . 

P(i | j ) is  also  called  the  state  covariance  of  x.  given  y , ....  /.. 

— ' _ - **  \J 

A. 2 Kalman  Estimation 

A recursive  procedure  for  realization  of  the  optimal  linear  estimator  for 
the  system  (1),  (2)  has  been  developed  by  Kalman  [3],  [4].  The  procedure 
consists  of  two  stages.  The  first  stage  employs  a filter  algorithm,  while 
the  second  uses  a smoother  algorithm**. 

2 T 

* The  notation  | |z| | = z z denotes  the  ordinary  Euclidean  norm  of  z. 

**The  terminology  employed  here  is  due  to  N.  Wiener  and  has  been  adopted  by  R. 
Kalman.  An  estinator  which  estimates  x^  given  measurements  with  indices  up 
to  and  including  j is  called  a f 1 1 ter  'if  1 = j;  it  is  called  a predictor 
if  1 > j,  and  it  is  called  a smoother  if  1 < j. 


Application  of  the  Kalman  filter  to  the  system  (1),  (2)  yields  x(i|i) 
and  P(i|i)  = P..  by  recursion.  The  algorithm  is 


(3) 

Ki  " 

p:hJ[h.p:h!  + R.]"1  , 

(4) 

A 

Xi  = 

xi  + K,.(y.  “ * 

(5) 

Pi  = 

PT  - K^PT,  i = 0,  1,  . 

(6) 

xi+l 

* Vi  ’ 

(7) 

p;+i 

= $iPi«T  + Qi,  i - o,  1, 

The  smoother  algorithm  is  initiated  when  the  filter  stage  is  complete.  The 

smoother  uses  the  filter  outputs  in  a recursive  process,  which  runs  in  reverse 

/\ 

order  to  the  filter  recursion,  to  compute  x(i|N)  and  P(i|N),  i =0,  1,  ....  N. 
The  algorithm  for  smoothing  is  given  by 


(8) 

Ai  = 

(9) 

x(i|N) 

= x.  + A.[x(i+1 |N)  - xT+i 

(10) 

P(i|N) 

= P1  + A1[P(1+1|N)  - p:+1 

, . . • , 0 


Carlson  Square  Root  Filter  Formulation 


There  are  several  algorithms  which  are  mathematically  equivalent  to  the 
Kalman  filter  algorithm,  but  which  recursively  compute  a square  root  S of 
the  state  covariance  matrix  P,  rather  than  the  covariance  matrix  itself 
[5],  [6],  [7].  In  each  of  these  algorithms,  the  condition 


P = SS‘  , 
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which  Is  the  defining  relation  for  the  square  root  of  P,  holds.  These 
algorithms  differ  markedly  because  of  the  nonuniqueness  of  S.  The 
distinctive  feature  of  the  Carlson  algorithm  is  that  S is  maintained  in 
triangular  form. 

The  Carlson  algorithm  is  initialized  by  applying  the  Cholesky  decomposition 
and,  if  necessary,  the  Gram  Schmidt  process  (Cf  Appendix  D)  to  P~  to  obtain 
the  appropriate  (i.e.,  upper  or  lower)  triangular  form  of  S”. 

The  Carlson  algorithm  will  be  given  for  the  case  where  the  measurement 
is  a scalar.  When  the  measurement  is  a vector,  the  Carlson  algorithm  can 
be  applied  sequentially  to  each  scalar  component  of  the  measurement  vector 
by  the  procedure  developed  in  Section  6.2. 


The  measurement  update  relations  for  the  Kalman  filter  are  given  by  (3),  (4), 
and  (5)  and  the  time  update,  or  extrapolation,  relations  are  given  by  (6) 
and  (7).  The  corresponding  update  and  extrapolate  relations  will  now  be 
developed  ffor  the  Carlson  square  root  filter. 

/\ 

Assume  that  xT,  ST,  and  y.  are  given,  where  ST  is  triangular  and  satisfies 


P-  = ST(ST)  . 

Assume  further  that  y^  is  a scalar  measurement  of  the  form 
*i  = h1xi  + vi  ’ 

where  v.  has  mean  zero  and  variance  r- . Then  the  Carlson  update  algorithm 

I A • 

can  be  used  to  compute  x^  and  S.,  where  S.  is  triangular  and  satisfies 


Vi  • pi  -= p;  - Kihipi  • 
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j 

The  Carlson  update  algorithm  will  be  derived  for  the  case  ir  which  S is 
maintained  in  upper  triangular  form.  However,  by  a simple  reversal  of 
index  order,  this  algorithm  can  also  be  used  to  maintain  S in  lower  tri- 
angular form. 

For  notational  brevity,  the  index  i will  be  dropped.  Thus 
y = hx  + v,  E(v)  = 0,  C(v)  = r , 
and 

4 

P~  = S‘(S")T  . 

Put 

g * (s‘)ThT 

and 

a * hP“fJ  + r * g^g  + r . 

A A 

If  a 3 0,  then  x « x"  and  S = S",  and  the  update  process  is  complete.  If 
a f 0,  it  follows  from  (3)  that 

(11)  K = (i)S'g  . 

Then  from  (4) 

(12)  x = x"  + K(y  - hx")  , 
and  from  (5) 

P = S"(S~)T  - (l)S"ggT(S’)T  , 
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or 


(13)  P = S'[I  - a_1ggT]  (S‘)T  . 

-1  T 

Now  let  U be  an  upper  triangular  square  root  of  [I  - a gg  ],  i.e.. 


[I  - cf]ggT]  * UUT  . 


Then  with 

(14)  S = S'U  , 
it  follows  that 

P = SST  . 

Since  the  product  of  two  upper  triangular  matrices  is  itself  upper  triangular, 
it  is  clear  that  S is  the  desired  form  of  the  updated  square  root  covariance. 

All  that  remains  to  complete  the  development  of  the  Carlson  update  procedure 
is  to  compute  U.  To  this  end  let 


gT  = (0,  ....  0,  g^,  ....  gn) 


where  0 < m < n-1.  (If  s 0 for  all  i,  then  U = I,  K = 0,  x = x",  and 
S = S'.) 

Then,  by  the  Cholesky  decomposition. 


I_  0 
m 


0 W 


H 


1 

f 

5 


that 


9j$j| ' 

Define 

pi+1  = pi  + 9isi*  1 = m+1»  •••»  n*  P-j  = 0,  ’ < m+1 

Then  s..  * c^sT  - d.p.. , i = 1,  ....  n . 

Moreover,  since 

Vl  ■ j,  * S'9  • 


and 


it  follows  from  (11)  that 
K = anlpn+l  ' 

From  the  above  development,  it  is  seen  that  the  Carlson  update  procedure  can 
be  carried  out  with  recursive  operations  on  the  columns  of  S.  An  auxiliary 
sequence  of  vectors  (p^ , i = 1,  ....  n+1)  is  computed,  and  the  Kalman  filter 

gain  is  given  by  K = o^p^. 

The  complete  algorithm,  valid  for  both  upper  and  lower  triangular  forms  is 
summarized  below.  For  the  upper  triangular  form  i ^ =1 , in=n,  and  id=+l, 
while  for  the  lower  triangular  form,  i^=n,  in*l,  and  id=-l.  Notice  that 
the  algorithm  includes  tests  to  prevent  divisions,  as  well  as  certain  mul- 
tiplications, by  zero. 


•i 

» 
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Carlson  Update  Algorithm 

9 = (9i » •••»  9p)  = hS 

2.  K = 0,  1p  = 0 

3.  a = r,  y =/a“ 

4.  i = i, 

5.  IF  (r  > 0)  60  TO  11 

6.  IF  (g?  > 0)  GO  TO  12 

7.  si  = s. 

8.  IF  (i  = in)  GO  TO  35 

9.  Hi  + id 

10.  GO  TO  6 

11.  IF  (g?  = 0)  PUT  si  = sT  AND  GO  TO  21 

12.  a = r + g? 

13.  8 = y 

1*.  y = /a" 

15.  IF  (e  > 0)  GO  TO  18 

16.  Si  = 0 

17.  GO  TO  20 

18.  C = 8/a 

19.  si  = c s~- 

20.  K = gis’. , ip  = 1 

21.  IF  (i=in)  GO  TO  34 

22.  i «-  i + id 

23.  IF  (g?  = 0)  PUT  s.  = sT  AND  GO  TO  33 

24.  a •*-  a + g? 

25.  8 = y 

26.  y s/a_ 
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To  develop  the  extrapolation  relations  for  the  Carlson  square  root  filter, 

A 

assume  that  x. , S.  are  given,  where  is  triangular  and  satisfies 


Assume  also  that 

xi+l  = Vi  + ui  ’ 

A 

where  u*  has  mean  zero  and  covariance  Q. . Then  xT+^  = $.x. , and  all  that 
is  required  of  the  Carlson  extrapolate  algorithm  is  to  compute  $T+^ , where 
ST+1  is  triangular  and  satisfies 

Si+l(Si+l^T  = Pi+1  = Vi*i  + Qi  * 

The  Carlson  extrapolate  procedure  is  based  on  the  Gram-Schmidt  orthogonal ization 
procedure  which  is  discussed  in  Appendix  D.  To  illustrate  the  procedure,  drop 
the  subscripts  for  brevity  and  let  the  dimension  of  S and  Q each  be  nxn. 

The  first  step  in  the  procedure  is  to  apply  the  Cholesky  decomposition  to 
obtain  a square  root  of  Q,  i.e., 

Q = rrT  . 

Then  the  nx2n  matrix 

[os  I r] 

is  formed.  Finally,  an  orthogonal  2nx2n  matrix  T is  determined  such  that 

(15)  [S'  | 0]  = [OS  | r]T 

where  S is  an  nxn  triangular  matrix.  Since 
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1- 


S‘(S')T  = [*S  I 

= C*s  | 

= #ssV  + rrT  = $P*T  + Q , 

it  is  clear  that  S"  is  the  desired  form  of  the  extrapolated  square  root 
covariance. 

Thus  all  that  remains  is  to  construct  an  orthogonal  matrix  T which  satisfies 
(15).  This  is  accomplished  by  application  of  the  Gram-Schmidt  process  to 
the  rows  of  |>S  | r],  augmented  as  necessary  by  the  rows  of  the  2nx2n 
identity  matrix  I2n,  to  obtain  a set  of  2n  orthonormal  vectors.  These 
orthonormal  vectors  then  form  the  columns  of  T. 

If  S"  is  to  be  upper  triangular,  the  Gram-Schmidt  process  is  applied  to  the 
rows  of 

rn  0 

0 !n 

$S  r 

in  the  order  from  bottom  to  top,  until  a complete  orthonormal  set  is  obtained*, 
to  form  the  columns  of  T in  the  order  n,  ....  1,  n+1,  ...,  2n. 

If  S~  is  to  be  lower  triangular,  the  orthonormalization  procedure  is  applied 
to  the  rows  of 

r 

*n  0 

0 ln 


r]TT 

r] 


sV 


sV 


i 


7 


Linearly  dependent  rows  are  simply  skipped  over. 


I 
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trie  tuiumn5  oi  i in  me  oraer  trom  iett  to  rignt. 


That  T satisfies  (15)  follows  by  construction.  Also,  it  should  be  noted 
that  at  most,  n columns  of  T need  be  computed  since  the  products  involving 
the  remaining  columns  of  T in  (15)  are  all  zero. 
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B.  VARIATIONAL  FORM  OF  THE  NAVIGATION  EQUATIONS 

In  this  appendix,  the  variational  form  of  the  navigation  equations  with 
three  degrees  of  freedom  (OOF)  will  be  developed.  The  reference  coordinate 
system  is  taken  as  earth  fixed  (EF),  and  the  3 DOF  are  the  EF  position 
coordinates  of  the  navigation  system.  Extension  of  the  results  in  this 
appendix  to  6 DOF  can  be  readily  accomplished  by  augmenting  the  three 
position  coordinates  with  the  three  Euler  angles  which  express  the  spatial 
attitude  of  the  navigation  system  with  respect  to  the  EF  coordinate  frame. 
Extension  to  6 DOF  is  required  when  the  navigation  system  is  of  the  "strap- 
down"  variety  commonly  used  to  provide  on-board  instrumentation  data  during 
reentry. 

Let  P denote  the  position  vector  of  the  navigation  system  relative  to  the 
earth  center  of  mass.  Denote  by  [jjr]j  anc*  t1me  derivat1 ves 

with  respect  to  inertial  and  earth  fixed  frames,  respectively.  Then 

(1,  P - VP,  ♦ Ap 

where  Ag(P)  is  the  acceleration  due  to  gravity  at  P,  and  Ap  is  the  acceler- 
ation of  the  navigation  system  due  to  all  forces  except  gravity. 

The  velocity  vector  of  the  navigation  system  relative  to  the  earth  is  de- 
fined by 

(2)  V E [^]E  PEP. 

But 


^1  P = ^E  P + a)xP 

= V + wxP  , 

where  u is  the  angular  velocity  vector  of  the  earth  with  respect  to  an 
inertial  frame. 


: 


5 

i 

:i 

i 
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Consequently 


— — - 


£ 

l 


t 

t 

V 


i 


t 


(3) 


where 


A 1 [3t3E  V 


(v  ♦ ™p) 

C^]E  (v  + wxP)  + u)x(V  + u»xP) 
V + 2wxV  + ujx(u)xP) 

A + AR(P,  V)  , 

= V 


and 

AR(P,  V)  = 2uixV  + ojx(a)xP)  . 

The  quantity  AR(P,  V),  due  to  earth  rotation,  is  the  sum  of  Coriolus  and 
centripetal  accelerations. 


Now  from  (1),  (2),  and  (3),  the  navigation  system  equations  of  motion  (EOM) 
are  expressed  by 

= V 

V = Ag(P)  - Ar(P,  V)  + Ap  , 
or 


(4)  P = V 

V = Aq(P,  V)  + Ap  , 

where 

A0(P,  V)  = Aq(P)  - Ar{P,  V)  . 
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B.l  Boost  Phase 

Let  t denote  the  time  at  which  the  navigation  system  is  initialized.  Then 
o 

the  solution  of  (4)  can  be  expressed  by 

p(t) i rtr  v(f)  i r°  i 

(5)  = / dt‘  + + 

V(t)  J A [P(f),  V(f)]  Vp(t)  V(tQ) 

L J t0  L J l J L J 

where 


The  navigation  system  constructs  a solution  of  the  form  given  in  (5)  using 
outputs  from  an  inertial  measuring  unit  (IMU)  which  senses  Ap  and  a clock 
which  measures  time. 

The  acceleration  sensed  by  the  IMU,  denoted  by  ApN,  is  given  by 
AFN(t)  = Ap(t)  + M[Ap( . ) , t]bj 

where  bj  is  the  vector  of  IMU  error  coefficients*  and  M is  a matrix  which 
is  a function  of  t and  a functional  of  (Ap(x),  tQ  < x £ t).  Cut  if  the 
IMU  accelerometers  are  of  the  integrating  variety,  the  output  of  the  IMU 
is  sensed  velocity  which  is  given  by 

VFN(t)  - / AFH(f)df  + bQ  + r(t)  , 

*o 

where  bQ  and  r(t)  are  the  respective  bias  and  zero  mean  random  conponents 
of  instrument  output  error.  Thus  for  integrating  accelerometers,  the  IMU 
output  is  given  by 


The  time  measured  by  the  navigation  clock  is  given  by 


(7)  9N(t)  * t + AtN(t)  = (1  + AtN).t 

where  t denotes  t-ue  time  (i.e.,  time  indicated  by  an  earth  fixed  master 
clock)  and  At^(t)  is  the  navigation  clock  error.  This  error  can  be  ex- 
pressed in  the  form 

(8)  AtN(t)  = mT(t)bc  , 


where  b is  the  vector  of  clock  error  coefficients  and  m is  a vector 
c 

function  of  t. 


If  the  IMU  and  navigation  clock  outputs  are  given  at  discrete  times  t^, 

i = 0,  1,  2 a navigation  solution  of  the  EOM  can  be  obtained  by 

numerical  integration.  Similarly  a nominal  solution  (about  which  the 
variational  equations  are  obtained)  can  be  obtained  by  applying  nominal 
corrections  to  the  IMU  and  clock  outputs  prior  to  integration.  Assuming 
the  integrator  truncation  error  is  negligible,  the  nominal  solution  is 

i expressed  by 

r 

I (9) 


p(t) 

[ 

v(t') 

~ 

0 

P(t0) 

- / 

t(t')dt'  + 

- 

+ 

V(t)_ 

i 

4- 

A0[P(t'),  V(t')]_ 

vF(t)_ 

f. 


where  Vp  and  t denote  the  nominal  corrected  values  of  Vp^  and  0^,  respec- 
tively, and  Aq  denotes  AQ  evaluated  using  the  nominal  geopotential  model. 
Observe  that 

t(t)  = (1  + AtN)_1o0N(t)  , 

or,  approximately, 

t(t)  a t + mT(t)<Sbc  , 


and  thus 
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t(t)  5 i + mT(t)6b£ 
Also,  note  that 


Vp(t)  3 VF(t)  + 6b0  + / M[Af(.),  t']  dt ’ 6bj  + 6r(t)  . 


The  geopotential  function  can  be  expressed  by 


(10)  Ar(P,  c)  = Aft(P,  c)  + 


0) 


(c  - c)  , 


where  c is  the  vector  of  true  geopotential  coefficients  and  c is  che  value 
of  these  coefficients  used  in  the  nominal  solution.  Thus 


and 


AJP,  V)  = Ar(P,  c)  - A0{P,  V)  , 


A0(p.  V)  = Ag(P,  c)  - Ar(P,  V)  = Aq(P 


6c  . 


The  solution  expressed  by  (9)  is  called  the  on-board  nominal  solution.  In 
order  to  relate  this  solution  to  off-board  measurements,  it  is  necessary  to 
use  the  navigation  clock  (corrected  for  nominal  errors)  and  construct  an 
off-board  nominal  solution.  The  off-board  solution  is  given  t.y 


(ID 


P'(t) 

_V'(t)_ 

t 1 t(tQ) 


The  variational  equations  will  be  developed  exclusively  for  the  on-board 
solution,  and  (11)  will  be  used  to  relate  on-board  trajectory  variations 
to  off-board  measurement  variations. 


In  order  to  obtain  the  variational  equations,  (9)  is  subtracted  from  (5) 
and  the  right  hand .side  of  the  resulting  difference  is  expanded  to  first 
order  about  the  nominal.  The  variational  equations  thus  obtained  are  given 
by 


i 

l 
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(12) 


0 


I 


6P(t  ) 


The  above  equation  expresses  the  trajectory  variations  in  integral  form. 
Differentiation  of  both  sides  and  augmentation  with  the  error  coefficient 
variations  yields  the  complete  set  of  navigation  variational  equations  in 
differential  form.  Thus 


Equation  (13)  is  exact  only  for  the  case  where  VpN  is  provided  continuously 
with  respect  to  time.  When  VpN  is  given  only  at  discrete  times,  the  form 
of  M and  r will  depend  on  the  method  of  interpolation  of  VpN  used  in  inte- 
grating the  EOM.  For  example,  suppose  Vp^  is  given  at  tQ,  t-j,  t£,  and 
linear  interpolation  is  used  in  the  intervals  [t..,  t.+j],  i = 0,  1,  2,  — 
That  is,  for  t.  < t < , i =0,  1,  2,  ...» 

t(t)  - t(t.)  1 « 

041  VF<tl  * w CVF(ti+1> ' vti)]  • 


Then  ignoring  velocity  interpolation  error, 


t(ti+1)  - t(t.)JL  t. 


M[Ap( . ) , t']dt' Sbj  + 6r(ti+1)  - 6r(t.) 


' Lt, 


and  therefore,  in  (13)  it  follows  that  the  substitutions 


(15)  M 


/ 1 \ fX  i 

\ Atl  / J 


M[Ap(.),  t']dt' 


(16)  6r"-^At7j[6r(ti+l)  ' 6r{ti,]  » 

where  Ati  = t.+,  - t..,  must  be  made  for  all  t in  the  interval  [t. , ti+-,]. 


M[Ap( . ) , t*  ]dt*  6bj  + <!r(ti+1)  - 5r(ti) 


Suppose  also  that  the  navigation  system  is  initialized  while  the  vehicle  is 
at  rest  on  the  launch  pad.  In  this  rase 

^FN^o^  = = ® » 


and  thus 


r<V 


I 


r 

* 

i 


i 


Furthermore,  if  Vp(tQ)  = 0 , 

«r(t0)  * -4b0  . 

Now  define  the  state  noise  process 

ui  = r^ti+l^’  i 2* 

and  introduce  the  correlated  noise  states  £ and  n defined  by: 
C(t)  = 0 , 
n(t)  = 0 , 


tn-  < t < t.+1  , and 


'c(tj)' 

0 I 

\(t:) 

0 

= 

+ 

1 

o 

o 

1 

n(tT) 

-V 

i = 0,  1,  2,  ....  with  initial  condition 


'o ' 

0 

Then  from  (16) 

- Silt)],  t1  < t < tj+1,  1-1,2 

irpn(t)  + »„],  t0  < t < t,  . 


Thus,  augmenting  the  variational  equations  with  the  correlated  noise  states, 
it  follows  that  (13)  can  be  expressed  by: 


for  i * 0,  1,  ....  with  initial  condition 


v(t;> 

MO 


p<t0) 

v<‘„> 


Le<t;,J  LCJ 

If  Uj,  i * 0,  1 , 2,  ....  is  a sequence  of  uncorrelated  randon;  vectors,  then 
equations  (17),  (18),  and  (19)  provide  the  desired  structure  for  application 
of  the  linear  estimation  results  discussed  in  Appendix  A and  Section  5.0. 

If,  on  the  other  hand,  the  sequence  of  random  vectors  is  correlated,  additional 
correlated  noise  states  must  be  augmented  to  the  system  given  by  (17),  (18), 
and  (19)  in  order  to  obtain  the  structure  required  for  the  estimator  to  be 
optimal . 

B.2  Free-Fall  Phase 


The  position  and  velocity  of  an  RV  at  deployment  can  be  computed  from  (9) 
using  vehicle  Euler  angles  and  their  rates  together  with  a separation  model 
based  on  energy  and  momentum  relations.  Following  release,  the  position 
and  velocity  of  the  RV  can  be  computed  using  an  equation  analogous  to  (9) 
with  VF  = 0.  Thus  if  tA  is  the  time  of  deployment. 


Now  (20)  is  the  nominal  on-board  solution  for  the  RV.  The  oniy  timing  error 
associated  with  this  solution  is  that  which  exists  at  td-  Thus  the  nominal 
off-board  solution  for  the  RV  is  given  by 

prv^  + *d  " *^d^ 

(2D  __  __  t > t(td)  . 

_ ^Ry(^).  _^RV^  + *d  “ ^d^. 

The  variational  equations  for  the  RV  on-board  solution  are  given  in  differential 
form  by 


with 


6PRv(V 

«p(td) 

ep 

= 

+ 

«V(td) 

• u « 

.V 

(23) 


where  £p  and  ey  are  due  to  attitude,  attitude  rate,  and  separation  errors 


at  deployment. 


B.3  Reentry  Phase  with  On-Board  Instrumentation 

If  the  RV  includes  on-board  instrumentation  for  use  during  reentry,  the 
procedure  used  for  the  boost  navigation  system  can  be  used  to  develop 
nominal  and  variational  equations  for  the  reentry  navigation  system. 
These  equations  will  be  initialized  by  the  free  fall  solution  at  or  near 
pierce  point,  and  the  initialization  procedure  will  be  subject  to  both 
the  free  fall  timing  error  and  the  reentry  navigation  clock  error. 


The  time  measured  by  the  reentry  clock  is  given  by 

0R(t)  = t + AtR(t)  = (1  + AtR)0t  , 

where  t is  true  time.  Correction  of  the  reentry  clock  for  nominal  errors 
yields 


and  if 


tR(t)  = (1  + AtR)  o0R(t)  , 


AtR(t)  = mR(t)bRc  , 


tR(t)  s t + mR(t)6bRc  . 


The  nominal  on-board  reentry  solution  is  initialized  at  time  tp  using  the 
nominal  off-board  freefall  solution  as  follows. 


PR<V 

V‘p> 


WV* 

VRv(tR<t p)* 


But  from  (21) 


WV  + td  ' t^td^ 


VRV(tR(tp))J  L WV  + *d  ' t^td^ 
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Therefore 


’w" 

WV 

+ 

WV 

.vy. 

. WV. 

. VV- 

WV 

+ 

WV 

. WV- 

. WV . 

cyy  - ‘P  + »d  - ‘<yi 


(n,R(tp)SbRc  ‘ ",T<td)4bc)  ' 


Consequently 


(25) 


' sp*<y' 

6WV 

’ WV ' 

_ 4 vy 

_ <SVRV^tp)  _ 

. vy . 

(mR(tp)6bRc  ' r,T(td)6bc)  • 


Observe  that  (24)  is  used  to  initialize  the  nominal  on-board  reentry  solution, 
while  (25)  is  used  to  reset  filter  estimates  and  covariances  (or  square  root 
covariances)  at  the  outset  of  reentry. 


Finally,  in  order  to  relate  the  reentry  navigation  solution  to  off-board  sensor 
measurements,  the  nominal  off-board  reentry  solution  is  required.  This  solution 
is  given  by 


(26) 


' y t> ' 

^(tR^t)) 

. »«“> . 
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C.  METRIC  SENSOR  SYSTEMS 

The  diagram  shown  in  Figure  C.l  depicts  the  general  structure  of  a metric 
sensor  system,  the  signal  flow  through  the  system,  and  the  sources  and  in- 
jection points  of  various  errors.  The  sensor  elements  generally  have  rather 
narrow  operational  limits,  and  for  this  reason,  it  is  necessary  to  control 
the  sensor  with  a servo  mechanism.  The  loop  which  includes  the  sensor  elements, 
servo  electronics,  feedback  elements  (and  in  some  cases  the  encoder)  performs 
a tracking  function  which  maintains  the  target  within  the  sensor  operating 
1 imi  ts. 

In  an  angle  tracking  system,  the  sensor  elements  consist  typically  of  a 
monopulse  receiver.  The  receiver  senses  the  apparent  displacement  of  the 
target  relative  to  the  receiver  boresite  (or  tracking  axis)  in  the  form  of 
two  angles.  The  servo  electronics  and  control  elements  contain  amplifiers, 
filters,  and  antenna  drive  motors.  The  inputs  to  this  block  are  the  sensed 
angles  out  of  the  receiver,  and  the  outputs  are  motor  shaft  angles.  The 
feedback  element  is  an  antenna  which  is  positioned  by  the  motors,  and  the 
sensor  Input  elements  are  rigidly  mounted  to  the  antenna.  Tne  motor  shaft 
angles  are  encoded  outside  the  loop,  i.e.,  the  encoders  are  not  within  the 
control  loop. 

In  a range  tracking  system,  the  sensor  element  is  typically  a device  which 
measures  the  time  difference  between  the  leading  edge,  centroid,  or  some 
other  point  on  the  received  pulse,  and  a timing  pulse  output  from  the  feed- 
back element.  The  sensor  element  output  Is  an  analog  signal  which  is  input 
to  the  servo  electronics  and  control  elements.  This  block  consists  of 
amplifiers,  filters,  and  Integrator  circuits.  The  output  of  this  block  is 
roughly  proportional  to  target  range  and  is  digitized  in  the  encoder  which 
is  typically  inside  the  control  loop.  The  feedback  element  produces  a 
timing  pulse  delayed  In  time  (relative  to  pulse  transmission  time)  by  an 
amount  controlled  by  the  encoder. 
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FIGURE  C.1  METRIC  SENSOR  SYSTEM  FLOW  DIAGRAM 
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In  a range  rate  or  doppler  system,  the  sensor  element  produces  a signal  with 
phase  equal  to  the  difference  of  the  phases  of  the  received  signal  and  a 
frequency  controlled  oscillator  in  the  feedback  element.  This  phase  modulated 
signal  is  input  to  the  servo  electronics  and  control  block  which  consists  of 
a frequency  discriminator,  amplifiers,  filters,  and  integrators.  The  output 
of  this  block  (vhich  is  roughly  proportional  to  the  doppler  shift  of  the 
received  signal  relative  to  the  transmitted  signal)  is  digitized  in  the  encoder. 
Then  depending  on  whether  the  oscillator  in  the  feedback  element  is  controlled 
by  an  analog  or  digital  signal,  either  the  servo  output  or  the  encoder  output 
is  input  to  the  feedback  element. 

In  a pulse  radar  system,  the  optimal  sensor  element  is  a matched  filter,  but 
in  practice  the  implementation  is  usually  a suboptima 1 approximation  to  a 
matched  filter.  The  output  of  the  sensor  element  (regardless  of  whether  it 
includes  a matched  filter)  consists  of  a sequence  of  pulses  v>hich  is  synchron- 
ous with  the  received  pulse  sequence.  This  sequence  of  pulses  contains  target 
information  corrupted  by  various  systematic  errors  and  sequentially  uncorrelated 
noise.  Since  the  information  in  this  pulse  sequence  is  relative  to  the  sensor 
track  point  which  is  monitored  by  the  encoder,  it  is  necessary  to  use  the  encoder 
measurements  to  relate  the  sensor  outputs  to  a reference  coordinate  system. 

C.l  Metric  Sensor  Measurement  Equations 

From  the  foregoing  discussion,  it  is  clear  that  the  sensor  element  (i.e.,  receiver) 
outputs  in  the  range,  doppler,  and  angle  channels  constitute  the  fundamental 
measurements  of  a metric  sensor,  while  the  encoders  merely  provide  the  means 
whereby  these  measurements  are  related  to  a reference  coordinate  system. 

Consider  a four  channel  tracking  radar,  and  let  y denote  the  measurement  vector 
at  time  t.  The  components  of  y are  simply  the  receiver  outputs  in  range,  doppler, 
and  angles  at  t.  Let  x denote  the  transit  time  of  the  signal  which  is  received 
at  t,  and  let  t'  denote  the  time  when  the  signal  left  the  target.  Then 

(1)  f - t-x  , 

and  it  is  the  target  position  and  velocity  at  t'  which  ultimately  affect  the 
measurements  mad?  at  time  t. 
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Let  the  rdnge,  azimuth,  and  elevation  of  the  target  be  defined  with  respect 
to  the  topocentric  coordinate  system  located  at  the  sensor  as  shown  in  Figure 
C.2.  Now  define  apparent  values  of  these  spherical  coordinates  to  be  equal 
to  true  values  modified  by  refraction. 


Thus 

(2) 

RA(f) 

= R(t')  + p[R(t'),  E(t')]  , 

(3) 

AA(f) 

= A(f)  , 

(4) 

EA(f) 

= E(t')  + e[R(t'),  E (t * )]  , 

where  the  apparent  values  are  subscripted,  true  values  are  unsubscripted,  and 
p and  e denote  range  and  elevation  refraction,  respectively.  Observe  that  for 
a specified  refractivity  between  target  and  sensor,  the  apparent  values  of 
target  position  relative  to  the  sensor  are  functions  only  of  true  position 
relative  to  the  sensor.  Note  also  that 


(5)  ct  = RA(t')  , 

and  thus  for  a given  t,  (1),  (2),  and  (5)  together  with  the  true  target 
trajectory  relative  to  the  sensor  suffice  to  determine  transit  time. 


The  apparent  range  rate  is  given  by 

(6)  RA(f)  = R(t')  + 

and  this  quantity  determines  apparent  doppler  by 

-#1 


1 ,dp 

R(t* ) 

[mjTj  v 

E(f) 

(7) 


DA(f)  = 


c + RA(t' ) 


where  f is  the  transmitter  frequency. 


S 


k 
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The  apparent  quantities  given  by  equations  (2),  (3),  (4),  and  (7),  possibly 
corrupted  by  target  dependent  error,  provide  the  inputs  which  generate  y. 
Thus  the  input  quantities  of  interest  are  the  received  values 

(8)  RR(  t)  = RA(f)  + ART(f)  , 

(9)  AR(t)  = AA(t‘)  + AAT(f ) , 


ER(t) 


EA(f)  + AET(f)  , 


(11)  DR(  t ) = DA(f)  + ADT(f)  , 

where  aR-j.  is  due  to  beacon  delay,  aAj  and  aEj  are  due  to  phase  front  distortion, 

and  aDt  is  due  to  beacon  oscillator  drift. 

In  the  range  channel,  the  sensor  element  forms  the  difference 

(12)  rQ(t)  = RR(t)  - Rp(t)  , 

where  Rp(t)  is  the  output  of  the  feedback  element.  Assuming  the  range  encoder 
is  inside  the  tracking  loop, 

(13)  Rp(t)  = RE(t)  - ARF(t)  , 

where  Rp(t)  is  the  encoder  output  and  ARp(t)  is  the  error  introduced  in  the 
feedback  path.  If  the  feedback  error  is  due  only  to  a bias  and  a scale  factor. 


(14)  ARp(t)  = BR  + SFr  . R£(t)  . 

Now  the  sensor  element  output  consists  of  the  range  difference  rQ  corrupted 
by  sensor  errors  and  receiver  noise.  Thus 

(15)  rs(t)  = rQ(t)  + Ar$(t)  + vr(t)  , 

where  Ar$(t)  is  the  error  introduced  by  the  sensor  and  vr(t)  is  toe  receiver 
noise  error.  If  the  sensor  error  is  due  only  to  a scale  factor,  then 
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(16)  Ar$(t)  = SFr  . rQ(t)  . 

In  the  doppler  channel  the  sensor  element  produces  a signal  with  phase  equal 
to  the  difference  in  the  phase  of  the  received  signal  and  the  phase  of  a 
frequency  element.  In  an  optimum  mechanization,  this  phase  difference  would 
be  measured  directly  and  would  constitute  one  component  of  the  measurement 
vector.  Typically,  however,  this  phase  modulated  signal  is  filtered  and 
input  to  a frequency  discriminator  which  forms  the  differenca 

(17)  dQ(t)  = Dr( t ) - Dp(t)  , 

where  Dp(t)  is  the  doppler  modulation  in  the  feedback  element.  Because  of 
the  filtering  operation,  the  frequencies  in  (17)  are  not  instantaneous 
quantities.  Instead  they  are  approximately  equal  to  the  average  frequencies 
over  a time  Interval  determined  by  filter  bandwidth.  (Actually,  if  the  filter 
and  discriminator  parameters  are  known,  a state  variable  representation  can 
be  used  to  relate  the  frequency  output  given  by  (17)  to  the  phase  difference 
of  the  received  signal  and  the  feedback  oscillator.  This  approach  will  not 
be  taken  here,  however. ) 

Now  assuming  the  doppler  encoder  is  inside  the  tracking  loop, 

(18)  Dp(t)  = 0£(t)  - ADp(t)  , 

where  D£(t)  is  the  encoder  output  and  ADp(t)  is  the  error  introduced  in  the 
feedback  path.  In  general  the  feedback  error  may  include  bias  and  scale 
factor  terms.  However,  because  Dp(t)  in  (17)  is  an  average  value  due  to 
filtering  In  the  sensor  element,  ADp(t)  will  also  include  a term  which 
accounts  for  the  average  lag  error  of  the  feedback  oscillator  in  response  to 
inputs  from  the  encoder.  In  a pulsed  doppler  radar,  with  period  T , in  which 
the  feedback  oscillator  is  reset  only  once  each  period,  the  average  lag  error* 
is  approximately  given  by  TfDR(t)/2  [8  ].  Thus  accounting  for  bias,  scale 
factor,  and  lag  errors. 


*The  existence  of  this  error  has  been  verified  both  experimentally  and 
analytically  for  the  SAMTEC  C-Band  systems. 


T . 

09)  ADF(t)  = Bd  + SFd  . D£(t)  + DR(t) 


The  frequency  discriminator  output  consists  of  dQ  corrupted  by  sensor  errors 
and  filtered  receiver  noise.  Thus 

(20)  d$(t)  = dQ(t)  + Ad$(t)  + vd(t)  , 

where  Ads(t)  is  the  error  introduced  by  the  sensor  and  vd(t)  is  receiver 
noise  error.  Assuming  the  sensor  error  is  due  only  to  a scale  factor, 

(21)  Ad$(t)  = SFd  . dQ(t)  . 

In  the  angle  channels,  angles  of  arrival  of  the  received  signal  with  respect 
to  the  tracking  axis  (i.e.,  electronic  boresite)  are  sensed.  In  order  to 
develop  expressions  for  the  sensed  angles,  it  is  convenient  to  first  define 
an  electronic  boresite  coordinate  system.  This  system  is  defined  by  a sequence 
of  rotations  applied  to  the  locally  level  (i.e.,  topocentric  coordinate  system 
located  at  the  sensor.  The  transformation  from  the  locally  level  system  to 

CD 

the  electronic  boresite  system  is  denoted  by  CEL(t). 

EB 

Assuming  the  angle  encoders  are  outside  the  loop,  CLL(t)  is  a function  of  the 
encoder  outputs,  encoder  errors,  and  feedback  errors.  This  functional 

CD 

representation  of  CLL(t)  will  now  be  developed. 

The  shaft  angles  of  the  tracking  system  are  related  to  the  encoder  angles  by 


(22) 


As(t) 

AE(t) 

AAE(t) 

Es(t) 

E£(t) 

AEE(t) 

where  AAE(t)  and  AEE(t)  are  the  encoder  errors,  which  include  bias  and 
nonlinearity  terms. 


The  azimuth  shaft  angle  measures  the  antenna  pedestal  rotation  in  a plane 
which  Is  determined  by  the  North  and  East  mislevel  coefficients,  yN  and  y£ 
These  coefficients  define  the  total  mislevel,  y,  and  the  azimuth  angle  of 
the  mislevel  axis,  Am,  by  means  of 
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(23) 


MCOsAm 

yE 

us1nAm 

The  transformation  from  the  locally  level  system  to  the  ml sieved  system  Is 
then  given  by 


(24) 


,ML 


'LL 


cosAm  sli-  0 

-sin^  cos^  0 
0 0 1 


cosy  0 -siny 
0 1 0 
Siny  0 cosy 


cos/^  -sinAm  0 
sinAm  cosA^  0 
0 0 1 


Now  an  azimuth  shaft  coordinate  system  is  defined  by  a rotation  of  the  mislevel 
system  through  the  angle  A<.(J;). 


This  rotation  is  given  by 


(25)  <4S(t)  = 


cosA^(t)  -sinAg(t)  0 
sinA$(t)  cosAs(t)  0 
0 0 1 


Next  a nonorthogonality  coordinate  system  is  defined  by  a rotation  of  the 
azimuth  shaft  system  through  an  angle,  n,  equal  to  the  nonorthogonality  of 
the  elevation  trunnion.  This  rotation  is  given  by 

“i 


(26)  C»(t) 


cosn  0 -slnn 
0 1 0 
sinn  0 cosn 


Now  an  elevation  shaft  coordinate  system  is  defined  by  a rotation  of  the 
nonorthogonality  system  through  the  elevation  shaft  angle  Es(t).  Thus 


(27)  cj£(t)  = 


NO' 


1 0 0 
0 cosEs(t)  s1nEs(t) 

0 -sinEs(t)  cosEs(t) 


ii 

,3 

, ;4 
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A mechanical  boresight  coordinate  system  Is  now  defined  In  terms  of  mis- 
alignment angles  In  traverse  and  elevation,  denoted  by  and  mf  respectively. 
Thus 


(28) 


1 0 0 

cosm^  -slnrrUj.  0 

rMB  _ 
LES  ' 

0 cosm^  sinm^. 

slnmj  cosm^  0 

0 -sinrn^  cosm£ 

0 0 1 

Next  a gravity  droop  coordinate  system  is  defined.  The  droop  angle,  6 is  a 
function  of  the  true  elevation  angle  of  the  mechanical  boresight,  and  it  is 
a rotation  abou;.  an  axis  which  lies  in  the  horizontal  plane  and  is  orthogonal 
to  the  mechanical  boresight.  The  transformation  from  the  mechanical  bore- 
sight to  the  gravity  droop  system  is  given  by 


cosy  0 -siny 
0 1 0 
siny  0 cosy 

where  y and  8 are  determined  as  follows. 

Define  unit  vectors  ex,  ey,  ey  by 


(29)  C<*  = 


cosy  0 siny 
0 1 0 
-siny  0 cosy 


fl  0 0 

0 cos6  s i n6 
0 -sin6  cos6 


If  |ev 


= 0,  then  8=0,  and  C 


GO 

MB  " 


I. 


Otherwise,  define 


ev  x e 
rv^i 


Then 


cosy  = ex  . eh  , 
siny  = (ex  x eh)  . ey  , 
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and  the  elevation  angle  of  the  mechanical  boresite  is  given  b> 


» 


E 

i 

I' 


with  -tt/2  < eMB  < ir/2.  Assuming  a simple  cantilever  model  for  droop,  it 
follows  that 


5 = 6qC0SEmb  , 

where  60  is  the  coefficient  of  droop. 


Finally  the  electronic  boresite  coordinate  system  is  defined  in  terms  of 
electrical  misalginment  angles  in  traverse,  elevation,  and  skew,  denoted 
by  ey.  eE,  and  e$,  respectively.  Thus 


COSGs 

0 

_s1nes 

1 

0 

0 

cose^ 

-sine^ 

0 

(30) 

cEB  = 
UGD 

0 

1 

0 

0 

coseE 

s1neE 

sine  j. 

coseT 

0 

_sinE$ 

0 

coses_ 

0 

-sineE 

coscE 

0 

0 

1 

The  transformati jn  from  the  locally  level  system  to  the  electronic  boresite 
system  is  given  by  the  product  of  successive  rotations.  Thus 


(31) 


rEB  rGD  rMB  rES  rN0  rAS  rML 
CGD  LMB  LES  °N0  UAS  TIL  LL  ' 


Let  e(t)  denote  the  unit  vector  defined  by  the  angles  of  arrival  of  the 
received  signal.  Expressed  In  the  topocentric,  or  locally  level  coordinate 
system,  this  unit  vector  is  simply 


(32)  eu(t)  = 


s1nAR(t)  cosER(t) 
cosAR(t)  cosER(t) 
sinER(t) 


Therefore,  in  the  electronic  boresite  system  this  unit  vector  is  given  by 


t 

] 
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eEetO 


eEB^ 


eJB(t)  sinAR(t)  cosER(t) 

eJR(t)  * C®tt)  cosAB(t)  cosEp(t) 


sinER(t) 


Now,  define  the  discriminator  traverse  and  elevation  angles  in  the  electronic 
bores Ite  system  by 


(34)  4>0(t)  = tan~1[e£B(t)/e^B(t)] 


(35)  d»0Ct)  = tan_1[egB(t)/e^B(t)]  , 


respectively.  Observe,  however,  that  these  angles  may  be  approximated  by 


(36)  4 (t)  = e£ 


EB(t ) 


(37)  *0(t)  = e*B(t)  , 

where  the  truncation  error  is  third  order.  Consequently,  if  <j>Q  and  are 
less  than  ten  milliradians,  the  errors  in  these  approximations  are  roughly 
one  microradian  or  less. 


The  outputs  of  the  sensor  element  are  given  by  <j>Q  and  corrupted  by  sensor 
errors  and  receiver  noise.  Assuming  the  sensor  errors  consist  of  scale  factor 
and  crosstalk,  the  sensed  angles  are  given  by 


♦sCt) 


M1) 


*o(t) 


SF**  [v0! 

+ + i 


sfm  sf# 


where  SF(j,«j,  and  SK#  are  scale  factor  errors,  SF  and  SF  are  cross  talk 
errors,  and  v^(t)  and  v^(t)  are  receiver  noise  errors. 
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C.2  Nominal  and  Variational  Measurement  Equations 

The  measurement  equations  for  a four  channel  tracking  radar  were  developed 
In  the  preceding  subsection.  If  the  sensor  element  outputs  at  time  t are 
denoted  by  r$(t),  <J>$(t),  <{>s(t),  and  d$ (t)  In  range,  traverse,  elevation,  and 
doppler,  respectively,  then  the  measurement  vector  is  defined  by 

'rs(tf 

(1)  y(t)  = $s(t)  . 

*s(t) 

ds(t) 

By  introducing  appropriate  state  variables  for  vehicle  dynamics  and  the 
various  vehicle  and  measurement  related  error  sources,  the  measurement  vector 
at  time  t can  be  expressed  in  the  following  algorithmic  form: 

(2)  t = 0s(t)  - mj(t)xd 

(3)  t'  - t - T 

(4)  ct  = R^(t') 

(5)  y(t)  = h[Xg(t'),  xb,  xc,  ....  xg,  z(t)]  + v(t)  . 

The  state  variables  which  appear  in  the  algorithm  are  defined  below, 
x'  - vehicle  trajectory  states  (off-board) 

a 

xb  - vehicle  dependent  measurement  error  states 

x„  - sensor  measurement  error  states 
c 

xd  - sensor  timing  error  states 

xg  - vehicle  timing  error  states 

Xf  - refraction  profile  states 

xg  - sensor  survey  states  (coordinates  of  geodetic 
position  and  astronomic  vertical) 
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In  equation  (2),  0$(t)  denotes  the  sensor  clock  Indicated  time  at  t,  and  ms(t) 
Is  a known  vector  function  of  t. 


Equation  (3)  expresses  the  relation  between  the  measurement  time  t,  transit 
time  t,  and  the  retarded  measurement  time  t1. 


The  transit  time  constraint  Is  expressed  In  equation  (4),  where  the  apparent 
range  R^(t* ) Is  uerived  from  x^(t'),  x^»  and  x^. 


Finally,  the  measurement  y(t)  is  expressed  by  (5),  in  which  z(t)  is  the 
encoder  measurement  vector  at  t,  and  v(t)  is  the  measurement  noise  vector 
at  t. 


The  complete  set  of  equations,  (2)  through  (5),  constitutes  the  dynamic 
measurement  algorithm,  while  equation  (5),  In  and  of  itself,  is  called  the 
static  measureme.it  algorithm. 


The  sequence  of  operations  in  the  dynamic  algorithm  is  as  follows.  First  the 
sensor  clock  measurement  is  corrected  to  obtain  t from  equation  (2).  Next 
equations  (3)  and  (4)  are  solved  Iteratively,  using  the  off-boa^d  trajectory 
x'  to  determine  t,  t',  and  x'(t').  Finally,  the  measurement  vector  y(t)  is 

a a 

obtained  from  equation  (5). 

If  nominal  values  of  the  state  variables  (including  a nominal  off-board 
trajectory  x')  are  specified,  the  dynamic  measurement  algorithm  can  be  used 

fl  -V  >V 

to  obtain  the  nominal  measurement  y(t),  computed  with  v(t)  s 1,  and  the 
partial  derivatives  which  appear  in  the  measurement  variation  equation: 

(6)  6y(t)  » 5x  ( t ’ ) + 6x.  + ...  + -^y  $x  + » 

ax!  a ax'  D ax'  9 

a b g 

where  6y(t)  = y(t)  - y(t),  and  the  partial  derivatives  are  evaluated  at  the 
nominal  state. 

The  relationship  which  exists  between  the  nominal  off-board  and  on-board 
trajectories  of  Appendix  B,  denoted  respectively  by  x'  and  x , can  be 

a « 

expressed  in  terms  of  the  vehicle  timing  variation.  Let  0v(t')  be  the 


149 


t 


vehicle  clock  indicated  time  at  t1,  and  suppose  that 

(7)  0y(t')  = t'  +mj(t')xe  , 

where  my(t')  is  a known  vector  function  of  t'.  Then  if  t'  is  the  nominal 
value  of  t'  obtained  from  the  measurement  algorithm,  it  follows  that 

(8)  ;;ff)  = xa(t'  - mj(t,)6xe)  . 

Observe  that  equation  (8)  indicates  the  mechanism  by  which  vehicle  timing 
variation  enters  the  measurement  variation  equation. 

Now,  the  partial  derivatives  in  equation  (6)  can  be  obtained  readily  by 
numerical  differentiation  using  the  dynamic  measurement  algorithm,  and  in 
many  instances  this  is  the  preferred  method.  However,  it  is  possible,  and 
sometimes  more  convenient,  to  express  the  measurement  parti a:  derivatives 
in  terms  of  partial  derivatives  of  the  function  h in  equation  (5).  The 
partial  derivatives  of  h with  respect  to  the  state  variable  elements  are 
called  static  measurement  partial  derivatives,  and,  when  clarification  is 
desired,  the  partial  derivatives  in  equation  (6)  are  called  dynamic 
measurement  partial  derivatives. 

The  remainder  of  this  subsection  is  devoted  to  expressing  the  dynamic 
measurement  partial  derivatives  in  terms  of  the  static  measurement  partial 
derivatives.  The  latter  can  of  course  be  obtained  by  numerical  differentiation 
using  the  static  measurement  algorithm. 


In  the  calculation  of  measurement  partial  derivatives  it  is  most  useful  to 
adopt  the  viewpoint  that  these  derivatives  are  of  y(t)  with  respect  to 
xa(f),  xb,  ....  Xg,  rather  than  derivatives  of  y(t)  with  respect  to 
xAV),  x.  , ....  x_,  evaluated  at  the  nominal  state.  (These  alternative 

a D g 

viewpoints  are  entirely  equivalent.)  Also,  for  the  sake  of  notational 
brevity,  the  (“)  notation  will  be  dropped  in  the  sequel  with  the  understanding 
that  it  applies  to  all  quantities  other  than  variations,  i.e.,  all  quantities, 
except  variations,  are  understood  to  be  nominal  values. 
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Now,  except  for  the  evaluation  of  the  partial  derivative  with  respect  to 
xg,  it  may  be  assumed,  because  of  (8),  that 

x;(f)  = xa(t!)  . 

Let  x^  be  any  component  of  the  nominal  state  vector,  except  a component  of 
xg.  That  is  x^  is  any  component  of  [xa(t‘),  xfa,  x£,  xd,  xf,  xg].  Then 
from  (5), 

(q)  ay  - ah  . ah  ; at1 

‘ ax,  »i  8T  xa  3x,  • 

a 


at1  _ at_  _ ar_ 
ax.  " ax..  " ax^ 


T ^jd 
\ ax..  ' 


T 3xd  3t 

s ax.  ax^ 


!^A  a atl 
ax^  A ax.. 


Therefore 


1 3RA  ; T 3xd 

77%  ^ ' A 5 **i  ’ 


and  thus 
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\ 


! 


I 


I 


do)  111  = . — ! — — e.  . 
v ' 3x  ' 


1 


3R, 


1 


c + Ra  9xi 


c + R. 


m 


J 

S 3x. 


From  (9)  and  (10),  using  the  notation 
3h 


H 


3x 


a 


it  follows  that 

(11) 

II 

J-y? 
<^|  ro 

Ha  - 

Haxa  3RA 
c + Ra  3xJ 

(12) 

SJF 

II 

Hb 

(13) 

II 

Hc 

(14) 

3JL  = 

.fl 

xa  T 

3xd 

c + 

ms 

ra 

(15) 

II 

Hf- 

_V,  9RA 

c + 3x  j. 

(16) 

_ 

V 

Ha*a  3RA 

. T 

c + R.  3x„ 

A g 

The  evaluation  of  the  partial  derivative  with  respect  to  xg  can  be  accomplished 
most  readily  by  indirect  means. 


First,  observe  that  if  8 denotes  a bias  in  the  vehicle  clock,  it  follows  by 
the  chain  rule  of  partial  differentiation  that 

'">  fr  ■ s < ■ 


I 


: 1 

■ I 
] 
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Then  from  the  measurement  algorithm  and  equation  (8),  observe  that  if  a bias 
a is  introduced  into  the  sensor  clock  and  an  equal  bias  8 is  introduced  Into 
the  vehicle  clock,  there  is  no  change  in  the  calculated  value  of  y.  (Recall 
from  Appendix  B that  a vehicle  clock  bias  has  no  affect  whatsoever  on  the 
on-board  nominal  trajectory  x_.)  Consequently 

a 

3a  38  U * 

and  it  follows  by  equations  (14)  and  (17)  that 

CH  x'  T 

(18)  \ • -M-  mj  . 

or;  v 

To  conclude  this  subsection,  the  complete  measurement  variation  equation, 
in  terms  of  static  partial  derivatives  and  with  complete  notation,  is  given 
for  reference.  i-  -i 

~ H x1  9R 

(19)  Sy(t)  = H *4-  -4  6x  (t1) 

a • aY*  a 

C + dXa 


+ Hk6x.  + H 6x_ 
DO  C C 


cH  x' 


54-  m Jft)6xd 


cH.x’ 


ml(t)6x,i  + -54-  mj(t'  )6x 


c + RA 


c + RA 


+ H 


+ H 


c + RA 


c + RA 


5xg  + v(t) 


C.3  Measurement  Processing  Using  Only  Encoder  Measurement 

The  mathematical  development  to  this  point  has  been  based  on  the  assumption 
that  measurements  at  both  the  sensor  element  output  and  the  encoder  are  made 


in  each  metric  information  channel.  In  many  cases  however  only  encoder  measure- 
ments are  made,  and  an  alternative  measurement  processing  scheme  must  be  employed. 

It  should  be  noted  at  the  outset  that  there  is  no  method  of  procersing  encoder 
measurements  alone  which  can  completely  compensate  for  the  lost  information 
carried  in  the  sensor  element  output.  If  encoder  measurements  alone  are  to  be 
processed  in  an  optimal  estimator,  the  best  that  can  be  done  is  to  augment  the 
estimator  state  vector  with  additional  dynamic  states  which  characterize  the 
servo  plant  ( i - e. , all  system  components  between  the  sensor  element  and  the 
encoder  input),  and  treat  the  encoder  output  as  the  fundamental  metric  sensor 
measurement.  But  this  approach  requires  explicit  knowledge  of  the  differential 
equations  for  the  servo  plant,  and  such  knowledge  is  not  always  readily  available. 
Moreover,  if  this  approach  were  used  for  every  sensor  channel,  the  number  of 
additional  states  would  greatly  increase  the  computational  burden  of  the  estimator. 
Consequently,  the  servo  state  augmentation  method  of  processing  encoder  measure- 
ments will  not  be  considered  further. 

A commonly  used  method  of  processing  encoder  measurements  (and  the  only  method 
which  will  be  given  further  consideration)  is  based  on  the  use  of  a set  of 
so-called  servo  dynamic  error  coefficients  to  approximate  the  quasi  steady 
state  following  (or  lag)  error  of  the  servo.  This  approximation  -s  then  used 
in  lieu  of  the  actual  sensor  element  output  in  the  measurement  processing 
operation. 

In  the  sequel,  a linear  model  for  a metric  tracking  system  will  be  used  to 
develop  the  theoretical  basis  for  the  use  of  dynamic  error  coefficients.  Then 
comparisons  will  be  made  between  three  alternative  schemes  in  which  the  measure- 
ments processed  consist  of,  respectively: 

(i)  both  sensor  element  and  encoder  outputs, 

(ii)  encoder  output  compensated  by  the  dynamic  error  coefficient 
approximation  to  sensor  element  output, 

(iii)  encoder  output  alone. 

It  will  be  seen  that  both  (ii)  and  (iii)  suffer  from  certain  deficiencies  in 
comparison  to  (i). 


A linear  model  for  a single  channel  metric  sensor  Is  depicted  In  Figure  C.3. 
The  variables  In  the  diagram  are  Identified  as  follows: 

xA  - apparent  value  of  channel  coordinate 

Ax-]-  - target  dependent  error 

xR  - received  (or  input)  value  of  coordinate 

Xp  - feedback  value  of  coordinate 

y - sensor  element  output 

Ay,.  - systematic  sensor  error 

v - measurement  noise 

AXp  - feedback  error 

xQ  - output  value  of  coordinate 

Axq  - output  error  including  servo  noise 

2 - encoder  output 

AXp  - encoder  error 

G(s)  - servo  plant  transfer  function,  i.e.,  Laplace  transform 
of  servo  plant  impulse  response. 

Although  the  diagram  is  drawn  for  the  case  where  the  encoder  la  outside  the 
tracking  loop,  it  can  be  applied  to  cases  where  the  encoder  is  inside  the  loop 
by  simply  equating  xQ  and  z and  combining  AxQ  and  AXp. 

From  the  diagram  It  follows  Immediately  that 

(1)  y + z = xA  + AxT  + AXp  + AXp  + Ay$  + v . 

Now  the  quantity  (y  + z)  may  be  regarded  as  the  sensor  measurement  when  both 
y and  z are  measured,  since  the  measurement  variations  obtained  from  either 
y or  (y  + z)  are  identical.  That  is 


<5(y  + z)  5 (y  + z)  - (y  + z)  = 6y  . 


fr 


The  expression  for  the  encoder  measurement  is  also  readily  obtained  from  the 
diagram.  Thus 

z = P*[xA  + AxT  + AxF  + Ay$  + v]  + q*Axft  + AxF  , 


o E 


2 = p*xA  + p * [AXy  + AXp  + Ay$]  + q*Ax0  + Ax^.  + p*v 


where 


><s>  - r+^fW  • 


Q(s)  • 


p(t)  = £ " {P(s)J  , 
q(t)  = £ _1  {Q(s ) > , 

g'1  denotes  the  inverse  Laplace  transform,  and  * denotes  the  convolution 
operation  defined  by 

00  00 

(p*x)(t)  = / p(u)x(t  - u)du  ■ / p(t  - u)x(u)du  . 

-co  -00 

The  constraints  imposed  on  servo  design  are  typically  such  that  in  (2)  p acts 
as  a low  pass  filter,  and  q acts  as  a high  pass  filter.  The  term  p*xA  is  a 
delayed  and  distorted  version  of  xft.  The  term  p*v  is  a smoothed  version  of  v 
which  exhibits  serial  correlation,  where  the  correlation  time  is  roughly  equal 
to  the  reciprocal  of  the  bandwidth  of  the  filter  p.  If,  as  is  the  usual  case, 

AXj,  AXp,  and  Ay$  are  very  low  frequency  in  character,  the  term  p*[AxT  + AXp  + Ays] 
is  approximately  just  [AxT  + Axp  + Ay$].  Since  q is  a high  pass  filter  the  term 
q*AxQ  will  contain  only  the  high  frequency  servo  noise  content  in  AxQ,  and  the 
low  frequency  servo  noise  will  be  rejected. 

Now  the  approximation  to  the  sensor  element  output  obtained  with  dynamic  error 
coefficients  will  be  derived.  From  the  diagram  of  Figure  C.3,  it  follows  that 


(•Mr 


(3)  y = q*[xA  + AxT  + AXp  - AxQ  + Ay$  + v]  . 

Since  q Is  a high  pass  filter,  while  AXy,  AXp  and  Ayg  are  typically  low  frequency 
terms,  it  follows  further  that 

(4)  y = q*[xA  - Ax0  + v]  . 

Deletion  of  the  noise  and  error  terms  In  (4)  leads  to  the  quantity 

(5)  y - q*xA 

which  contains  all  the  information  in  y regarding  xA.  The  procedure  is  now 
aimed  toward  approximation  of  y. 

The  expression  for  Q(s)  is  first  expanded  in  a Taylor  series  about  the  origin. 
Thus 


Qts)  . 


' + *0  K1 


2 3 

+ i_  + + ii 

+ + + If  + • • • , 


where  is  the  dynamic  error  coefficient  of  order  i for  i = 0,  1,  2 
If  the  function  G(s)  has  a pole  of  order  n at  the  origin,  then 


Ki  = 


>,  1 = 0,  1,  2,  ...,  n-1  , 


lim  snG(s),  i = n . 


In  this  case  the  system  in  Figure  C.3  Is  said  to  be  of  type  n. 

Type  0 systems  are  of  little  interest.  In  fact,  most  metric  tracking  systems 
are  either  type  1 or  type  2.  If  type  0 systems  are  excluded,  then  KQ  = » and 


m q(s)  ■!-  + £-  + §-+  .... 

K1  *2  K3 

* • 

Using  (7)  In  (5)  It  follows  from  elementry  properties  of  Laplace  transforms 
that 
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y<t)  - 


xA(t)  xA(t) 

■Tq-  * ~*r  * 


X.(t) 


At  this  point  the  assumption  is  made  that  xA  Is  a sufficiently  smooth  function 
that  the  series  In  (8)  can  be  truncated  after  the  second  or  third  term  with 
negligible  error.  Thus  assuming  a nominal  trajectory  Is  specified  for  xA,  the 
resulting  approximation  for  y is 


7(t)  = 


*A<‘> 


Sifttt) 

*2 


Xfl(t) 


Combining  (9)  with  (2)  there  results 


y + z = p*x.  + tA  + 


*A  + XA 

h k3 


+ p*[AXy  + AXp  + Ay$]  + q*Ax0  + AxE  + p*v  . 

At  this  point  the  three  measurement  processing  schemes  can  be  compared  to 
determine  their  relative  merits.  The  first  alternative,  which  is  feasible 
only  when  both  encoder  and  sensor  element  outputs  are  available,  provides  a 
measurement  expressed  by  (1).  In  this  case  the  measurement  has  a simple 
linear  dependence  on  the  measured  coordinate,  systematic  error  te-ms,  and 
measurement  noise.  The  systematic  error  terms  can  be  expressed  as  linear 
functions  of  state  variables,  and  the  measurement  noise  in  the  sensor  element 
is  sequentially  uncorrelated.  Thus  the  measurement  given  by  M)  satisfies 
all  assumptions  required  In  the  development  of  the  optimal  estimation  equations 
in  Appendix  A. 

The  next  alternative  to  be  considered  is  the  case  in  which  the  uncompensated 
encoder  output  is  used,  and  the  measurement  is  expressed  by  (2).  In  this  case, 
as  has  been  mentioned,  the  measured  coordinate  is  delayed  and  distorted  by 
filtering,  the  measurement  contains  high  frequency  servo  noise,  and  the  noise 
which  originated  in  the  sensor  element  or  receiver  now  appears  as  smoothed 
and  sequentially  correlated  measurement  noise.  It  is  clear  that  the  measure- 
ment in  this  case  does  not  satisfy  the  assumptions  required  by  ihe  estimator 
for  optimality,  and  some  degradation  in  estimator  performance  will  result. 


,:?A.  <k * ft fwafc  1 1^  ****  '•* 


The  last  alternative  Is  the  case  in  which  the  encoder  output  is  compensated 
using  dynamic  error  coefficients,  and  the  measurement  Is  given  by  (10).  Com- 
parison of  (2)  with  (10)  shows  that  all  the  objectionable  features  present 
in  (2)  are  also  present  in  (10),  except  that,  to  some  extent  in  the  latter, 
the  delay  in  xA  las  been  compensated  by  the  introduction  of  the  dynamic 
error  terms. 


The  application  of  dynamic  error  coefficients  in  the  single  channel  range 
and  doppler  trackers  is  straight  forward.  Each  of  these  trackers  is  typically 
of  type  2.  Thus  the  dynamic  error  terms  are  given  by 


r = 


RA  ]£ 

;r  ,r 


and 


a = -4 


da  ^ 

«D  + „D 
*2  *3 


respectively,  where  and  are  derived  from  the  off-board  nominal  solution. 


The  application  of  dynamic  error  coefficients  in  angle  trackers  must  account 
for  the  fact  that  the  sensing  element  output  which  drives  the  azimuth  servo 
senses  angle  in  the  traverse  plane.  Consequently  the  gain  of  the  azimuth 


servo  is  multiplied  by  a factor  G(E$)  to  ensure  uniform  servo  response  in 


azimuth  for  all  elevation  angles  except  those  in  a small  region  naar  zenith. 


E$  is  the  elevation  shaft  angle,  and 


G(ES)  > 


sec  E$,  Es  < Emax  , 


sec  Emax*  ^ax  4 Es  £ ' 


For  operation  with  E > tt/2,  G(E  ) = -G(ir  - E_). 

j m 
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The  dynamic  error  approximations  In  traverse  and  elevation  are  given  by 


D.  MISCELLANEOUS  TOPICS  IN  LINEAR  ALGEBRA 

\ 

The  results  In  this  appendix  can  be  found  In  many  standard  mathematics  and 
numerical  analysis  texts  [9],  [10],  [11].  They  are  Included  her*  to  provide 
a concise  and  readily  accessible  supplement  to  the  algorithms  developed  In 
the  text. 

D.1  Gauss  Elimination 

Let  A be  an  nxn  matrix.  Assume  for  the  moment  that  A has  full  rank.  The 
Gauss  elimination  procedure  applied  to  A consists  of  a sequence  cf  operations 
on  the  rows  of  A which  yields  an  upper  triangular  matrix  U.  To  be  specific, 
let 


a A = 


a(°)  a(o) 
all  a12 

a(o)  a(o) 
a21  a22 


a(o)  a(o)  a(o) 
anl  an2  • • * ann 


Using  the  elements  of  the  first  column  of  A'0'  define 


i 


Then  let 


P 


Then  with 


j 

i 


and  let 


Then 


where 


and  in 


a(1)  ■ l;'  ...  . 


A'* 'has  the  structure 


0 


*a 


(i-1) 

ii 


0 


,(1)  = ..(1-D 


kj 


aO-l)a(i-l) 
aki  a-' 


kj 


j»  k ■ i+l 


aii 


particular 


WW  S*u  ■ u • : '-i-t**'  Jx> 


Using  the  definitions  introduced  above,  the  Gaussian  elimination  procedure 
can  be  represented  in  matrix  form  by 

(1)  L-\  ...  L^A  = U . 

Now  let  e^  denote  the  i-th  column  of  the  nxn  identity  matrix  and  for  each 
i = 1,  ....  n-1 , let  X j denote  the  n-vector  defined  by 


xi  = 


^i +1.1 


Then  for  each  i = 1 , . . . , n-1 , 


I - A.e.  , 


and  it  is  obvious  that 


1-1  = * + Aiei 


Finally  let 


L s L1  •”  Ln-1  ’ 


and  observe  that 


l = i + e xie;. 


Then  from  (1)  it  follows  that 


A = LU  , 


where 


I 
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(8) 


L 


0 

0 


r 


1 0 ...  . 


(8) 

L = 

C\J 

1 

0 . . 

• 

0 

*32 

• 

• 

• 

• 

• 

• 

0 

_*nl 

*n2 

/ 

• • • 

n 

»n-l 

1 

U11 

u12 

• • • 

uln 

~ a(°) 
all 

a(o) 

‘ • aln 

0 

u22 

• • • 

u2n 

0 

a(1) 

a22.  • * * • 

a*1* 

• a2n 

(9) 

U = 

o’ 

• 

= 

: 

o • 

• • 

• 

• 

* 

0 . 

. .’o 

unm  _ 

0 

• 

• 

0 . . . . . 

'o  -a'"-1’ 
nn 

Since  an  explicit  expression  for  L"1  does  not  appear  in  the  sequel,  it  is 
perhaps  worth  while  to  note  that 


n-1 

L f I - E x.ej  , 
n=l 

and  hence  L"1  cannot  be  obtained  by  simply  changing  the  sign  of  the  elements 
below  the  main  diagonal  in  (8). 

The  Gauss  elimination  procedure  has  led  to  the  factorization  of  A into  a 
product  of  triangular  matrices  in  (7).  This  form  is  particularly  useful  in 
solving  equations  of  the  form 

(10)  Ax  = b . 

A two  step  process  is  used  to  solve  (10)  for  x.  From  (7)  it  follows  that  (10) 
is  equivalent  to  the  pair  of  equations 
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1 

i 4 


(11) 

Ly  - b , 

(12) 

Ux  = y . 

Now  it  is  a simple  matter,  because  of  triangularity,  to  first  solve  (11)  for 
y by  forward  substitution  and  then  solve  (12)  for  x by  backward  substitution. 


In  the  course  of  the  Gauss  elimination  process,  the  columns  of  L 3re  formed 

sequentially  using  the  results  of  previous  calculations.  Thus  the  i-th 

column  of  L requires  division  by  the  diagonal  element  a(|”^.  If  a^-1^ 

fi-ll  11  11 

is  zero  the  process  fails  outright.  And  if  al^  'is  small  in  magnitude, 
it  can  contain  a relatively  large  roundoff  error.  In  such  cares  the 
relative  error  in  the  i-th  column  of  L,  and  hence  in  successive  computations, 
can  be  quite  large. 


Since  A is  nonsingular,  there  must  exist  a nonzero  element  ajk 


(1-1) 


for  some 

j i and  k 2:  i-  This  fact  provides  the  basis  for  a means  not  only  of 
avoiding  division  by  zero,  but  also  of  reducing  the  amount  of  roundoff  error 
buildup  in  Gauss  elimination.  The  procedure  is  called  pivoting,  and  the 
idea  is  to  interchange  rows  and  columns  in  the  object  matrix  to  bring  an 
element  of  large  magnitude  into  the  i-th  diagonal  position  prior  to  computing 
the  i-th  column  of  L.  The  larger  this  element,  relative  to  other  elements 
of  the  matrix,  the  smaller  will  be  the  roundoff  error  in  the  i-th  column  of 
L and  in  subsequent  calculations. 

The  theoretical  justification  for  the  pivoting  process  will  now  be  given. 

To  this  end,  the  notion  of  a permutation  matrix  is  formulated. 

An  nxn  matrix  P is  called  a permutation  matrix,  if  for  any  n-vector  x, 
the  vector  y defined  by 

y = Px 

is  obtained  by  interchanging  at  most  two  elements  of  x.  Thus  either,  y = x, 
or  there  exist  two  distinct  integers  j and  k such  that 
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!xk,  1*j  , 

Xy  1“k  , 

otherwise  . 


It  is  easily  established  that  if  P is  a permutation  matrix,  then  it  is 
symmetric  and  orthogonal,  i.e. , 

P = PT  = P"1  . 

Observe  also  that,  if  P is  an  nxn  permutation  matrix  and  B is  an  arbitrary 
nxn  matrix,  then 

PB 

is  a matrix  which  differs  from  B by  at  most  an  interchange  of  two  rows, 
while 

BP 

differs  from  B by  at  most  an  interchange  of  two  columns. 

To  illustrate  the  use  of  pivoting  in  Gauss  elimination,  the  raw  and  column 
interchange  operations  will  be  represented  by  permutation  matrices.  For 
each  i = 1,  ....  n-1,  just  prior  to  calculation  of  the  elements  in  LT\ 
a row  permutation  R-  and  a column  permutation  C.  are  performed  on  the  object 
matrix  to  place  an  element  of  large  magnitude  in  the  i-th  position  on  the 
diagonal.  To  preserve  triangularity  it  is  necessary  and  sufficient  that 
R.j  permute  rows  i and  j and  C..  permute  columns  i and  k,  where  j ■>  i and 
k > i.  Thus  the  row  and  column  Interchanges  bring  the  element  in  position 
(j,  k)  to  position  (i,  i)  In  the  object  matrix. 

In  matrix  form,  the  sequence  of  elimination  operations  can  be  represented 
as  follows 
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L“1R1AC1  = A^ 

[;ViVlC2  - A(2) 


*-n-lRn-r - -Cl  1rtaci  — cn_i  * a*"  ’* 


Now  with  the  definitions 


J = Rn_i* ••R1AC1...cn_1  , 
U . A(n'1}  , 


L = Rn-r  ••R2LlR2L2"*Rn-1Ln-l  * 


it  follows  that 


A = LU  . 


But  by  induction 


Rn-1* ••R2L]R2L2'**Rn-lLn-l 


Rn-1  * * *R2hR2^R3-  * *Rn-l  ^Rn-1  * * 'R3^2*  ’ *Rn-l^i-l 
[Rn-1  • ' *R2hR2-  ’ ,Rn-l ^Rn-1  * ' -R34*  * 'Rn-l^n-l ^ 

C1  + Rn- 1 ' * * R2*l e 1 ^Rn- 1 * * * R3*"2R3^3  * * * Rn-  l*“n-l  ^ 

= [i  + Rn_1...R2X1e{]  ...  [I  + Rn_^2eTn_2][I  + V-jeJ.')]  * 


where 


L = 


[I  + Xie{][I  + X2eJ] 


• H + Vlen-1] 


n-l 

I 

1=1 


i + i x^T  , 


( Rn-V ‘•Ri+lAi*  1 = ]*  ••••  n-2 

x,  = • 


f'i 


n-l 


, 1 = n-l 


A simple  bookkeeping  procedure  for  factorization  of  A into  LU  is  now  obvious. 
On  the  i-th  elimination  the  entire  nxn  object  matrix  is  subjected  to  the 
appropriate  row  and  column  permutation.  Then  the  elements  of  X^  are  computed 
and  the  elimination  operation  is  performed.  Finally  the  nonzero  elements  of 


X.  are  placed  in  the  corresponding  eliminated  positions  in  the  i-th  column 
below  the  main  diagonal.  That  is  is  placed  in  position  (k,  i)  for  each 
k = i+1,  ...»  n.  After  the  (n-l)-th  elimination,  the  object  matrix  is  of 
the  form 


The  solution  of  equation  (10)  using  Gauss  eliminations  with  pivoting  is 
quite  direct.  First  observe  that  the  system  (10)  is  equivalent  to 

(10')  fix  = b , 

where 

* = Rn-1*  •*RlAcr  *,Cn-l  ’ 

b = Rp_i  • • • R'j  ^ 5 

and 

x = cn-r,,cix  • 

But  (10')  can  be  solved  using  the  factorization 
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I = LIT 


by  solving  the  equivalent  system 
(IT)  = b , 

(12')  Ox  = y , 

\ 

I from  which  x is  obtained  simply  by 

x = C-j . . • Cn_-jX 

The  pivot  procedure  described  above  is  sometimes  called  complete  Pivoting. 

In  the  special  cases  in  which  either  R-|  = ...  = Rn_-|  * I or  C-j  ■ ..  = Cn_^  = I, 
the  procedure  is  called  partial  pivoting. 

If  the  matrix  A is  of  rank  r < n,  then  of  course  A is  singular.  In  this 
case  the  Gauss  elimination  process  with  pivoting  will  terminate  immediately 
after  the  r-th  elimination  and  the  object  matrix  will  be  of  the  form 


U11 

U12 

• * • 

ulr  * * ' uln 

\ 1 

u22 

• # • 

^2r  * * * u2n 

^31 

^32 

■ 

• • 

• • 

• • 

Zr1 

CNJ 

7 

* ' *Y,r-l 

• • 

• 

urr  ’ * * urn 

• 

• 

• 

Vl.r  °’m'° 

• 

■ 

• 

• • • 

• • • 

*h1 

\z  ■ 

7 

• ’ n,r-l 

r °--*° 

n,r 
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D.2 


Cholesky  Decomposition 


A real  valued  nxn  matrix  A is  said  to  be  nonneqative  if  A is  symmetric  and 
the  condition 

xTAx  0 

holds  for  every  n- vector  x. 

A real  valued  nxn  matrix  S which  satisfies  the  condition 


l.W 


is  called  a square  root  of 


A. 


It  is  obvious  that  the  matrix  product  SS^  is  nonnegative  for  any  real  valued 
nxn  matrix  S.  Conversely,  as  will  be  shown,  if  A is  a nonnegativt  matrix 
then  a real  valued  square  root  of  A exists,  but  is  not  unique.  The  proof 
of  the  existence  of  a square  root  of  a nonnegative  matrix  will  be  by  con- 
struction using  Gauss  elimination.  This  constructive  process  is  also  called 
Cholesky  decomposition. 


Let  A be  a nonnegative  nxn  matrix.  It  is  readily  shown  that 


(i)  akk  > 0 for  all  k = 1,  ...,  n 
and 

(ii)  a = max  {akk;  k = 1,  ....  n}  implies  a = max  {|ajk|;  Jj  k = 1 n}  , 

that  is,  all  diagonal  elements  of  A are  nonnegative,  and  no  element  of  A has 
a magnitude  greater  than  the  maximal  diagonal  value  a. 

Gauss  elimination  with  pivoting  will  now  be  applied  to  A,  and  the  first 
permutation  matrices  R-j  and  C.j  will  be  selected  such  that 


[ 

! 


I 


i 


I 

I 

I 


and  since  A is  nonnegative,  must  also  be  nonnegative.  But  since  B^ 
is  nonnegative  it  follows  by  induction  that  the  Gauss  elimination  process 
can  be  continued  with  R.  = Ci  = P..  chosen  at  each  stage  to  place  a maximal 
value  element,  from  the  remaining  diagonal  elements,  in  the  (i,  i)  position. 
If  for  some  iQ  < n,  the  remaining  diagonal  elements  are  all  zero,  take 
A.j  = 0 and  R.j  = = P^  = I for  all  i = iQ,  ...»  n-1. 

Thus  when  A is  nonnegative,  the  sequence  of  elimination  operations  can  be 
performed  with  symmetric  pivoting  and  represented  as  follows. 

L^1P]AP1  = A(1) 

[2V;'wz  * 


L»Vn-l'"Lll¥Pl-Pll-l  " *ln  ^ 


Then  with  the  definition 


* * Pn-r**PlAPr  **Pn-l  * 


U = A 


(n-1) 


^ 5 Pn-T”P2LlP2L2,,,Pn-lLn-l  ’ 


it  follows  that 


S’  * LU  . 


But  it  is  also  true  that 


l-',A(L-1)T  . U(L-')T  = D , 


which  implies  D Is  nonnegative  (hence  symmetric)  and  upper  triangular.  Thus 
F is  a nonnegative  diagonal  matrix. 
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Now  let  denote  the  (unique)  nonnegative  diagonal  matrix  which  is  a 
square  root  of  15,  and  put 


I = 5S-1  , 

and  thus  if  is  a lower  triangular  square  root  of  Ji.  It  follows  immediately 
that 

A =*  SST  , 


where 


S ~ P1 — Pn-lS  * 

Thus  S is  a square  root  of  A,  and  the  construction  is  complete.  Note  that 
S,  unlike  5,  Is  not  necessarily  triangular.  A method  of  transforming  S into 
a triangular  square  root  of  A using  the  Gram-Schmidt  process  Is  described 
In  0.3. 

When  the  Cholesky  decomposition  Is  applied  to  a nonnegative  matrix  A,  a 
simplified  bookkeeping  procedure  can  be  used  which  takes  advantage  of 
symmetry.  First,  recall  that 

D * U(L-1)T  • 

Then  since  U has  unity  diagonal  values  so  does  L"1.  Consequently,  D is 
simply  the  diagonal  part  of  U,  l.e. , 


177 


D = 


Thus  since 


un  °. 


. 0 


o 

0 ...  0 u 


nn 


U = EC 


it  follows  that 


ujk  = for  k > j,  j = 1 n-1  . 


Now  if  A is  of  rank  r,  then  u.  • > 0 for  j < r and  u...  = 0 for  j > r, 

J J J J 


j ~ 1 j . . . i n. 


But  if  A is  of  rank  r,  it  also  follows  that  = 0 for  k > j > r Consequent- 


ly, the  entries  in  L below  the  main  diagonal  are  determined  from  U by 


r — 
u 


'kj 


0 , j > r , 
for  j < k < n,  j = 1 , ....  n-1. 


. j i r , 


Second,  recall  that 


s = nr 


■1/2 


Then  if  A is  of  rank  r,  the  elements  of  S on  and  below  the  main  diagonal  are 
given  by 


i 


Skj  - * 


V“jj 


o , j > r , 
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for  j < k < n , j * 1 , . . . , 


n. 


Since  T and  5 are  readily  computed  from  U,  the  Cholesky  decomposition  can 
be  carried  out  without  bothering  to  compute  and  store  L.  Furthermore,  be- 
cause of  symmetry  the  entire  process  can  be  performed  using  only  the  upper 
triangle  of  the  object  matrix.  Also,  the  square  root  normalizations  used 
to  obtain  S from  U can  be  performed  in  conjunction  with  the  eliminations, 
so  that  the  object  matrix  contains  5*  (rather  than  H)  when  the  process  is 
complete.  Conceptually,  the  bookkeeping  method  used  in  Cholesky  decomposition 
differs  from  that  given  for  Gauss  elimination  as  follows: 


Only  symmetric  pivoting  is  used,  and  is  chosen  to  bring  a 
maximal  valued  element,  from  the  remaining  diagonal  elements, 
into  position  (i,  i). 

Instead  of  computing  , k = i+1,  ...,  n,  for  the  i-th  elimination, 
the  elements  in  the  i-th  row  of  the  object  matrix  are  normalized  by 
the  square  root  of  the  diagonal  element  in  that  row.  That  is 


~(i-D 

aik 


;o-i) 

aik 

:(t-d 

aii 


k * i n . 


The  i-th  elimination  is  performed  as  follows: 


a(t^  = 0,  j * i+1 n 

J * 


,( i ) . :d-i) 

*jk  " ajk  aij  aik 


av;/  = " - ai:  'a n ’ , j < k < n,  j = i+1 , ...»  n 


and 


akj}  = aili)*  J < k 1 J s 1+1 n-1  • 


jk 


When  all  elimination  steps  are  complete  and  the  object  matrix  contains  ST. 
Thus  if  A is  of  rank  r,  the  object  matrix  contains 


i 


■j 

i 
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S11  * • • srl  * ’ • snl 
0 . 


0 • • • 0 s . • • s 
rr  nr 


0 ....  0 


0.  . .0  0 ....  0 


If  the  nonnegative  matrix  A satisfies  the  stronger  condition. 


xTAx  > 0 


| 

1 


for  every  nonzero  n-vector,  then  A is  positive.  In  this  case,  Cholesky 
decomposition  can  be  applied  without  pivoting,  theoretically,  although  it 
may  be  inadvisable  to  do  so  because  of  roundoff  error  buildup.  Nonetheless, 
if  pivoting  is  not  used,  a simple  formula  for  S,  due  originally  to  Cholesky, 
follows  by  induction  from  the  Gauss  elimination  method,  modified  as  above  to 
take  advantage  of  symmetry.  The  formula  is 


5ii  " faii  ' 1 sik)  2 » 1 = 1 n • 

\ l<k<i  / 

5ji  = sTrAji  ' z sjksikV  J = i+1 1 = 1 n*1 

\ / 


As  a final  observation,  it  is  noted  that  the  Cholesky  decomposition  can  be 
applied  to  obtain 

* = SST  , 

where  S is  upper  triangular.  All  that  is  required  is  that  the  row  elimination 
operations  be  applied  to  the  object  matrix  from  the  right  (instead  of  left) 
and  proceed  from  bottom  to  top  (rather  than  top  to  bottom). 


a 


i 


1 


i 

J. 
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0.3 


Sram-Schmidt  Orthonormalization 


The  Gram-Schmldt  orthonormallzatlon  process  Is  a method  of  constructing  an 
orthonormal  set  of  vectors  from  an  arbitrary  set  of  linearly  Independent 
vectors.  In  thi:  appendix*  the  treatment  is  limited  to  real  valued  n-vectors. 
For  a complete  treatment,  the  reader  is  referred  to  [12],  [13]. 

The  inner  product  of  two  real  n-vectors  x and  y is  given  by 

(x,  y)  = xTy  = yTx  = (y,  x)  , 

and  x and  y are  said  to  be  orthogonal  if 

(x»  y)  = 0 • 

The  norm  of  an  arbitrary  vector  x is  defined  by 
I |x| I * (x,  x)1/2  . 

A set  of  vectors  . ..,  ^ is  said  to  be  linearly  independent  if  the 
relation 

Vi  * •••  * v.  ■ 0 

holds  only  when  c.j  = ...  = cm  = 0. 

A set  of  vectors  u-,,  ....  u_  is  said  to  be  orthonormal  if 
I m 

!1  i - j , 

0 i i»*  J , 

if  j “ 1*  • « • t m* 


rr 


f 

I 

l 

t 

i 

\ 

i 

i- 


!' 

c 


1 


A set  of  vectors  which  is  orthonormal  is  necessarily  also  linearly  indepen- 
dent, but  the  converse  does  not  hold.  The  Gram-Schroidt  algorithm  for 
constructing  an  orthonormal  set  of  vectors  from  an  arbitrary  linearly 
independent  set  of  vectors  x-j,  ....  xm  is  developed  below. 

Define  a set  of  vectors  y-|,  . ...  y as  follows 


*1  = X1 

y2  = x2  - (x2,  yj) 

yi  = x1  - (xi#  y1 ) 


*1 


*1 


|yi 


<V  *1-1  > 


'i-1 


ri-l 


= % - <v  »i>  tztz  - ■■■  ■ <v  vi>  Vl 


I |y,  1 1 


^m-l 


It  follows,  by  linear  independence,  that  ) ]y. | | > 0,  i » 1,  .... 


m. 


It  is  easily  shown,  by  induction,  that  y-j , ....  ym  is  an  orthogonal  set  of 
vectors.  Furthermore,  for  each  i =1,  ....  m,  the  set  y^,  . ..,  y^  and  the 
set  x-|,  ....  x.j  span  the  same  subspace,  that  is  every  vector  which  can  be 
expressed  as  a linear  combination  of  members  of  one  set  can  also  be  ex- 
pressed as  a linear  combination  of  members  of  the  other  set. 


Now  define  the  set  u-| um  by  normalization  of  the  set  y-j , ....  ym, 

that  is. 


ui  = 


, i — 1,  . . . , m . 


Then  u^,  ...,  um  is  an  orthonormal  set  of  vectors,  and  for  each  i = 1,  . 
the  sets  x^,  ...,  x^  and  u^,  . ...  span  the  same  subspace. 
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The  above  algorithm  for  obtaining  the  set  u-j,  ....  um  is  the  ordinary  Gram- 
Schmidt  process.  For  computational  reasons,  a slightly  modified  form  of  the 
algorithm  is  preferred.  This  algorithm,  which  is  called  the  modified  Gram- 
Schmidt  process,  is  given  below.  The  modified  algorithm  is  entirely 
equivalent  to  the  ordinary  algorithm,  theoretically,  but  is  numerically 
more  accurate  when  executed  on  a computer. 

xj1)  = X|,  i*l,  ...,i 


= I ly-,  1 1 


<x{”  1 


• • ) m 


y2/l |y2 


,(k)  _ v(k-l)  ,v(k-l) 


(xi  t i = k*  • • . 9 m 
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To  illustrate  an  application  of  the  Gram-Schmidt  process  which  is  of 
particular  interest,  let  S be  an  arbitrary  square  root  of  an  nxn  non- 
negative matrix  A,  that  is 

A = SST  . 

Now  denote  the  rows  of  S by  the  set  of  n-vectors  xj,  ...»  x^.  Thus 


Apply  the  Gram-Schmidt  process  to  the  set  of  vectors  x-j , ....  xn,  augmented 
if  necessary  by  the  columns  of  the  nxn  identity  to  obtain  the  orthonormal 
set  u-j , ....  un. 

Augmentation  with  columns  of  In  is  necessary  only  if  Xp  ....  xn  is  not 
linearly  independent.  In  this  case  the  Gram-Schmidt  process  is  carried 
out  as  before,  skipping  any  vector  x^  which  is  linearly  dependent  on  the 
set  Xp  ....  x._y  Only  when  the  original  set  is  exhausted  dees  augmentation 
occur.  Thus,  in  general,  the  orthonormalization  process  is  applied  to  the 
set  Xp  ...,  xn,  e^ , ...,  en,  and  terminates  when  a complete  orthonormal 
set  Up  ...,  un  is  obtained. 

Now  let  T be  the  orthogonal  matrix  whose  columns  are  given  by  the  set 
u^,  . . . , un>  Thus 


T = Lu1|...|un]  . 
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